Python rospy 模块，logfatal() 实例源码

def update_telemetry(navsat_msg, compass_msg):
    """Telemetry subscription callback.

    Args:
        navsat_msg: sensor_msgs/NavSatFix message.
        compass_msg: std_msgs/Float64 message in degrees.
    """
    try:
        client.post_telemetry(navsat_msg, compass_msg)
    except (ConnectionError, Timeout) as e:
        rospy.logwarn(e)
        return
    except (ValueError, HTTPError) as e:
        rospy.logerr(e)
        return
    except Exception as e:
        rospy.logfatal(e)
        return

def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing");

def publish_obstacles(timer_event):
    """Requests and publishes obstacles.

    Args:
        timer_event: ROS TimerEvent.
    """
    try:
        moving_obstacles, stationary_obstacles = client.get_obstacles(
            frame, lifetime)
    except (ConnectionError, Timeout) as e:
        rospy.logwarn(e)
        return
    except (ValueError, HTTPError) as e:
        rospy.logerr(e)
        return
    except Exception as e:
        rospy.logfatal(e)
        return

    moving_pub.publish(moving_obstacles)
    stationary_pub.publish(stationary_obstacles)

def delete_object(self, req):
        """Handles DeleteObject service requests.

        Args:
            req: DeleteObjectRequest message.

        Returns:
            DeleteObjectResponse.
        """
        response = interop.srv.DeleteObjectResponse()

        try:
            self.objects_dir.delete_object(req.id)
        except (KeyError, OSError) as e:
            rospy.logerr("Could not delete object: {}".format(e))
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            response.success = True

        return response

def delete_object_image(self, req):
        """Handles DeleteObjectImage service requests.

        Args:
            req: DeleteObjectImageRequest message.

        Returns:
            DeleteObjectImageResponse.
        """
        response = interop.srv.DeleteObjectImageResponse()

        try:
            self.objects_dir.delete_object_image(req.id)
        except (KeyError, IOError) as e:
            rospy.logerr("Could not delete object image: {}".format(e))
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            response.success = True

        return response

def get_mission_by_id(req):
    """ Service to update mission information with specific mission as given
    by id.

    Args:
        req: GetMissionByID type request with field id corresponding to the
             mission

    Returns:
        GetMissionByIdResponse which is true for success and false for
        failure.
    """
    with lock:
        global msgs
        try:
            msgs = client.get_mission(req.id, frame)
        except (ConnectionError, Timeout) as e:
            rospy.logwarn(e)
            return False, str(e)
        except (ValueError, HTTPError) as e:
            rospy.logerr(e)
            return False, str(e)
        except Exception as e:
            rospy.logfatal(e)
            return False, str(e)

    rospy.loginfo("Using mission ID: %d", req.id)
    return True, "Success"

def add_object(self, req):
        """Handles AddObject service requests.

        Args:
            req: AddObjectRequest message.

        Returns:
            AddObjectResponse.
        """
        response = interop.srv.AddObjectResponse()

        dict_object = serializers.ObjectSerializer.from_msg(req.object)
        json_object = json.dumps(dict_object)

        try:
            file_id = self.objects_dir.add_object(json_object)
        except IOError as e:
            rospy.logerr(e)
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            response.id = file_id
            response.success = True

        return response

def get_object(self, req):
        """Handles GetObject service requests.

        Args:
            req: GetObjectRequest message.

        Returns:
            GetObjectResponse.
        """
        response = interop.srv.GetObjectResponse()

        try:
            json_object = self.objects_dir.get_object(req.id)
        except (KeyError, IOError) as e:
            rospy.logerr("Could not get object: {}".format(e))
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            dict_object = json.loads(json_object)
            response.object = serializers.ObjectSerializer.from_dict(
                dict_object)
            response.success = True

        return response

def get_all_objects(self, req):
        """Handles GetAllObjects service requests.

        Args:
            req: GetAllObjectsRequest message.

        Returns:
            GetAllObjectsResponse.
        """
        response = interop.srv.GetAllObjectsResponse()

        try:
            json_objects = self.objects_dir.get_all_objects()
        except IOError as e:
            rospy.logerr("Could not get all objects: {}".format(e))
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            for str_file_id, json_object in json_objects.iteritems():
                file_id = int(str_file_id)
                dict_object = json.loads(json_object)
                ros_object = serializers.ObjectSerializer.from_dict(dict_object)

                response.ids.append(file_id)
                response.objects.append(ros_object)

            response.success = True

        return response

def set_object_image(self, req, compress=False):
        """Handles SetObjectImage service requests.

        Args:
            req: SetObjectImageRequest/SetObjectCompressedImageRequest message.
            compress: Whether to return a compressed image or not.

        Returns:
            SetObjectImageResponse or SetObjectCompressedImageResponse.
        """
        if compress:
            response = interop.srv.SetObjectCompressedImageResponse()
        else:
            response = interop.srv.SetObjectImageResponse()

        try:
            png_image = serializers.ObjectImageSerializer.from_msg(req.image)
        except CvBridgeError as e:
            rospy.logerr(e)
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            try:
                self.objects_dir.set_object_image(req.id, png_image)
            except (KeyError, IOError) as e:
                rospy.logerr("Could not set object image: {}".format(e))
                response.success = False
            else:
                response.success = True

        return response

def get_object_image(self, req, compress=False):
        """Handles GetObjectImage service requests.

        Args:
            req: GetObjectImageRequest/GetObjectCompressedImageRequest message.
            compress: Whether to return a compressed image or not.

        Returns:
            GetObjectImageResponse or GetObjectCompressedImageResponse.
        """
        if compress:
            response = interop.srv.GetObjectCompressedImageResponse()
        else:
            response = interop.srv.GetObjectImageResponse()

        try:
            png = self.objects_dir.get_object_image(req.id)
        except (KeyError, IOError) as e:
            rospy.logerr("Could not get object image: {}".format(e))
            response.success = False
        except Exception as e:
            rospy.logfatal(e)
            response.success = False
        else:
            try:
                response.image = serializers.ObjectImageSerializer.from_raw(
                    png, compress)
            except CvBridgeError as e:
                rospy.logerr(e)
                response.success = False
            else:
                response.success = True

        return response

def fatal(self, msg):
        rospy.logfatal(msg)

def logfatal(msg):
    _var.logger.fatal(msg)

def bag_to_csv(options, fname):
    try:
        bag = rosbag.Bag(fname)
        streamdict= dict()
        stime = None
        if options.start_time:
            stime = rospy.Time(options.start_time)
        etime = None
        if options.end_time:
            etime = rospy.Time(options.end_time)
    except Exception as e:
        rospy.logfatal('failed to load bag file: %s', e)
        exit(1)

    try:
        for topic, msg, time in bag.read_messages(topics=options.topic_names,
                                                  start_time=stime,
                                                  end_time=etime):
            if streamdict.has_key(topic):
                stream = streamdict[topic]
            else:
                stream = open(format_csv_filename(options.output_file_format, fname[fname.rfind('/'):-4]+topic),'w')
                streamdict[topic] = stream
                # header
                if options.header:
                    stream.write("time")
                    message_type_to_csv(stream, msg)
                    stream.write('\n')

            stream.write(datetime.fromtimestamp(time.to_time()).strftime('%Y/%m/%d/%H:%M:%S.%f'))
            message_to_csv(stream, msg, flatten=not options.header)
            stream.write('\n')
        [s.close for s in streamdict.values()]
    except Exception as e:
        rospy.logwarn("fail: %s", e)
    finally:
        bag.close()

def joystickUpdated(data):
    rospy.logfatal("Emergency stop butoon pressed!")
    if data.buttons[0] == 1:
        for e in emergencies:
            try:
                e()
            except ServiceException:
                pass

def __init__(self):
        self.lock = threading.Lock()
        rospy.init_node("roboclaw_node",log_level=rospy.DEBUG)
        rospy.on_shutdown(self.shutdown)
        rospy.loginfo("Connecting to roboclaw")

        self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1)
        self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1)

        self.address = int(rospy.get_param("~address", "128"))
        if self.address > 0x87 or self.address < 0x80:
            rospy.logfatal("Address out of range")
            rospy.signal_shutdown("Address out of range")

        # TODO need someway to check if address is correct


        self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0"))
        self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0"))
        self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2"))
        self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
        self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1"))

        self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
        self.last_set_speed_time = rospy.get_rostime()

        rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1)

        rospy.sleep(1)

        rospy.logdebug("address %d", self.address)
        rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED)
        rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED)
        rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
        rospy.logdebug("base_width %f", self.BASE_WIDTH)

def reset(self):
        """
        Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable
        the robot.
        """
        error_not_stopped = """\
Robot is not in a Error State. Cannot perform Reset.
"""
        error_estop = """\
E-Stop is ASSERTED. Disengage E-Stop and then reset the robot.
"""
        error_nonfatal = """Non-fatal Robot Error on reset.
Robot reset cleared stopped state and robot can be enabled, but a non-fatal
error persists. Check diagnostics or rethink.log for more info.
"""
        error_env = """Failed to reset robot.
Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set
and resolvable. For more information please visit:
http://sdk.rethinkrobotics.com/intera/SDK_Shell
"""


        is_reset = lambda: (self._state.stopped == False and
                            self._state.error == False and
                            self._state.estop_button == 0 and
                            self._state.estop_source == 0)
        pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10)

        if (not self._state.stopped):
            rospy.logfatal(error_not_stopped)
            raise IOError(errno.EREMOTEIO, "Failed to Reset due to lack of Error State.")

        if (self._state.stopped and
              self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED):
            rospy.logfatal(error_estop)
            raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged")

        rospy.loginfo("Resetting robot...")
        try:
            intera_dataflow.wait_for(
                test=is_reset,
                timeout=5.0,
                timeout_msg=error_env,
                body=pub.publish
            )
        except OSError, e:
            if e.errno == errno.ETIMEDOUT:
                if self._state.error == True and self._state.stopped == False:
                    rospy.logwarn(error_nonfatal)
                    return False
            raise