Python rospy.ROSException方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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!") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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() --------------------------------------------------------------------
# ============================================================================== 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import ROSException [as 别名]
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"