Python rospy.wait_for_service() 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 __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 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 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 __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 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 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 pause_simulation(self):
        # rospy.wait_for_service('/gazebo/pause_physics')
        self.pause_sim() 

def resume_simulation(self):
        # rospy.wait_for_service('/gazebo/unpause_physics')
        self.unpause_sim() 

def reset_robot(self):
        # rospy.wait_for_service('/gazebo/reset_simulation')
        self.reset_simulation() 

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 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 execute(self, inputs, outputs, gvm):
    turtle_name = inputs["turtle_name"]
    x = inputs["x_pos"]
    y = inputs["y_pos"]
    phi = inputs["phi"]
    service = "/" + turtle_name + "/teleport_absolute"
    rospy.wait_for_service(service)
    move_turtle = rospy.ServiceProxy(service, TeleportAbsolute)
    resp1 = move_turtle(x, y, phi)
    self.logger.info("ROS external module: executed the {} service".format(service))
    return 0 

def execute(self, inputs, outputs, gvm):

        # check if the roscore is already running
        try:
            rospy.wait_for_service("/rosout/get_loggers", 5.0)
        except Exception as e:
            self.logger.error("Exception: " + str(e) + str(traceback.format_exc()))
            return -1

        node_name = inputs["node_name"]
        if type(node_name).__name__ == "unicode":
            node_name = node_name.encode('ascii', 'ignore')
            # print node_name
                    
        self.logger.info("Creating node: " + node_name)
        try:
            if not gvm.variable_exist("ros_node_initialized") or not gvm.get_variable("ros_node_initialized"):
                gvm.set_variable("ros_node_initialized", True)
                rospy.init_node(node_name, anonymous=True, disable_signals=True)
                listener = tf.TransformListener()
                self.logger.info("Creating node: " + str(listener))
                gvm.set_variable("tf_listener", listener,per_reference=True)
            if not gvm.variable_exist("robot_id"):
                try: 
                    gvm.set_variable("robot_id",rospy.get_param("robot_id"))
                except KeyError as e:
                    self.logger.warn("robot_id does not exist in the parameter server")

        except Exception as e:
            self.logger.error("Unexpected error:" + str(e) + str(traceback.format_exc()))

        return 0 

def _load_gazebo_urdf_model(cls,
                                model_name,
                                model_pose,
                                model_path,
                                model_reference_frame='world'):
        # Load target URDF
        with open(model_path, 'r') as model_file:
            model_xml = model_file.read()
        # Spawn Target URDF
        rospy.wait_for_service('/gazebo/spawn_urdf_model')
        cls.spawn_urdf(model_name, model_xml, '/', model_pose,
                       model_reference_frame) 

def initial_setup():
    """ Prepares the Gazebo world for generating training data.

        In particular, this routine turns off gravity, so that the objects
        spawned in front of the RGBD camera will not fall. It also deletes
        the ground plane, so that the only depth points produce will
        correspond to the object of interest (eliminating the need for
        clustering and segmentation as part of the trianing process)

        Args: None

        Returns: None
    """
    rospy.wait_for_service('gazebo/get_model_state')
    rospy.wait_for_service('gazebo/set_model_state')
    rospy.wait_for_service('gazebo/get_physics_properties')
    rospy.wait_for_service('gazebo/set_physics_properties')
    rospy.wait_for_service('gazebo/spawn_sdf_model')

    get_physics_properties_prox = rospy.ServiceProxy('gazebo/get_physics_properties', GetPhysicsProperties)
    physics_properties = get_physics_properties_prox()

    physics_properties.gravity.z = 0

    set_physics_properties_prox = rospy.ServiceProxy('gazebo/set_physics_properties', SetPhysicsProperties)
    set_physics_properties_prox(physics_properties.time_step,
                                physics_properties.max_update_rate,
                                physics_properties.gravity,
                                physics_properties.ode_config)


    delete_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
    delete_model_prox('ground_plane') 

def send_led_state(state):
    rospy.wait_for_service('/niryo_one/rpi/set_led_state')
    try:
        set_led = rospy.ServiceProxy('/niryo_one/rpi/set_led_state', SetInt)
        set_led(state)
    except rospy.ServiceException, e:
        rospy.logwarn("Could not call set_led_state service") 

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_motors_command():
    rospy.loginfo("Send reboot motor command")
    try:
        rospy.wait_for_service('/niryo_one/reboot_motors', 1)
    except rospy.ROSException, e:
        pass 

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_hotspot_command():
    rospy.loginfo("HOTSPOT")
    send_led_state(LedState.WAIT_HOTSPOT)
    rospy.wait_for_service('/niryo_one/wifi/set_hotspot')
    try:
        set_hotspot = rospy.ServiceProxy('/niryo_one/wifi/set_hotspot', SetInt)
        set_hotspot()
    except rospy.ServiceException, e:
        rospy.logwarn("Could not call set_hotspot service") 

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 activate_learning_mode(activate):
        try:
            rospy.wait_for_service('/niryo_one/activate_learning_mode', 0.1)
            activate_learning_mode = rospy.ServiceProxy('/niryo_one/activate_learning_mode', SetInt)
            activate_learning_mode(int(activate))
        except (rospy.ServiceException, rospy.ROSException), e:
            return False 

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 can_enable():
        rospy.wait_for_service('/niryo_one/commander/is_active')
        try:
            is_active = rospy.ServiceProxy('/niryo_one/commander/is_active', GetInt)
            response = is_active()
            return response.value == 0
        except rospy.ServiceException, e:
            return False 

def load_module(self, module_name):
        rospy.wait_for_service('/niryo_one/jevois/set_module')
        try:
            load_module = rospy.ServiceProxy(
                '/niryo_one/jevois/set_module', SetString)
            load_module(module_name)
        except rospy.ServiceException as e:
            rospy.logerr(str(e))