C++ Updater::force_update方法代码示例

  ViconReceiver() :
    nh_priv("~"), diag_updater(), min_freq(0.1), max_freq(1000),
        freq_status(diagnostic_updater::FrequencyStatusParam(&min_freq, &max_freq)), StreamMode("ClientPullPreFetch"),
        HostName(""), SubjectName(""), SegmentName(""), tf_ref_frame_id("/enu"),
        tf_tracked_frame_id("/pelican1/flyer_vicon"), update_rate(100), vicon_capture_rate(50),
        max_period_between_updates(0), max_abs_jitter(0), lastFrameNumber(0), frameCount(0), droppedFrameCount(0),
        frame_datum(0), latest_time_bias(0), time_bias_reset_count(0), n_markers(0), n_unlabeled_markers(0),
        time_bias_reset(0.05), segment_data_enabled(false), marker_data_enabled(false),
        unlabeled_marker_data_enabled(false), enable_tf_broadcast(false)

  {
    // Diagnostics
    diag_updater.add("ViconReceiver Status", this, &ViconReceiver::diagnostics);
    diag_updater.add(freq_status);
    diag_updater.setHardwareID("none");
    diag_updater.force_update();
    // Parameters
    nh_priv.param("stream_mode", StreamMode, StreamMode);
    nh_priv.param("datastream_hostport", HostName, HostName);
    nh_priv.param("subject_name", SubjectName, SubjectName);
    nh_priv.param("segment_name", SegmentName, SegmentName);
    nh_priv.param("update_rate", update_rate, update_rate);
    nh_priv.param("vicon_capture_rate", vicon_capture_rate, vicon_capture_rate);
    nh_priv.param("tf_ref_frame_id", tf_ref_frame_id, tf_ref_frame_id);
    nh_priv.param("tf_tracked_frame_id", tf_tracked_frame_id, tf_tracked_frame_id);
    nh_priv.param("time_bias_reset", time_bias_reset, time_bias_reset);
    nh_priv.param("enable_tf_broadcast", enable_tf_broadcast, enable_tf_broadcast);
    ROS_ASSERT(init_vicon());
    // Service Server
    ROS_INFO("setting up grab_vicon_pose service server ... ");
    m_grab_vicon_pose_service_server = nh_priv.advertiseService("grab_vicon_pose",
                                                                &ViconReceiver::grab_vicon_pose_callback, this);
    // Publishers
    pose_pub = nh_priv.advertise<geometry_msgs::TransformStamped> ("output", 10);
    markers_pub = nh_priv.advertise<vicon_mocap::Markers> ("markers", 10);
    // Timer
    double update_timer_period = 1 / update_rate;
    min_freq = 0.95 * vicon_capture_rate;
    max_freq = 1.05 * vicon_capture_rate;
    updateTimer = nh.createTimer(ros::Duration(update_timer_period), &ViconReceiver::updateCallback, this);
  }

  ViconReceiver() :
    nh_priv("~"), diag_updater(), min_freq_(0.1), max_freq_(1000),
        freq_status_(diagnostic_updater::FrequencyStatusParam(&min_freq_, &max_freq_)), stream_mode_("ClientPull"),
        host_name_(""), tf_ref_frame_id_("/world"), tracked_frame_suffix_("vicon"), broadcast_tf_(true),
        lastFrameNumber(0), frameCount(0), droppedFrameCount(0), frame_datum(0), n_markers(0), n_unlabeled_markers(0),
        marker_data_enabled(false), unlabeled_marker_data_enabled(false), grab_frames_(false)

  {
    // Diagnostics
    diag_updater.add("ViconReceiver Status", this, &ViconReceiver::diagnostics);
    diag_updater.add(freq_status_);
    diag_updater.setHardwareID("none");
    diag_updater.force_update();
    // Parameters
    nh_priv.param("stream_mode", stream_mode_, stream_mode_);
    nh_priv.param("datastream_hostport", host_name_, host_name_);
    nh_priv.param("tf_ref_frame_id", tf_ref_frame_id_, tf_ref_frame_id_);
    nh_priv.param("broadcast_tf", broadcast_tf_, broadcast_tf_);
    ROS_ASSERT(init_vicon());
    // Service Server
    ROS_INFO("setting up grab_vicon_pose service server ... ");
    m_grab_vicon_pose_service_server = nh_priv.advertiseService("grab_vicon_pose", &ViconReceiver::grabPoseCallback,
                                                                this);

    ROS_INFO("setting up segment calibration service server ... ");
    calibrate_segment_server_ = nh_priv.advertiseService("calibrate_segment", &ViconReceiver::calibrateSegmentCallback,
                                                         this);

    // Publishers
    marker_pub_ = nh.advertise<vicon_bridge::Markers> (tracked_frame_suffix_ + "/markers", 10);

    if (broadcast_tf_)
      tf_broadcaster_ = new tf::TransformBroadcaster();

    startGrabbing();
  }

  ///\brief Opens joystick port, reads from port and publishes while node is active
  int main(int argc, char **argv)
  {
    diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
    diagnostic_.setHardwareID("none");

    // Parameters
    ros::NodeHandle nh_param("~");
    pub_ = nh_.advertise<fmMsgs::Joy>("joy", 1);
    nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
    nh_param.param<double>("deadzone", deadzone_, 0.05);
    nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
    nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
    
    // Checks on parameters
    if (autorepeat_rate_ > 1 / coalesce_interval_)
      ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);
    
    if (deadzone_ >= 1)
    {
      ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
      deadzone_ /= 32767;
    }
    
    if (deadzone_ > 0.9)
    {
      ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
      deadzone_ = 0.9;
    }
    
    if (deadzone_ < 0)
    {
      ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
      deadzone_ = 0;
    }

    if (autorepeat_rate_ < 0)
    {
      ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
      autorepeat_rate_ = 0;
    }
    
    if (coalesce_interval_ < 0)
    {
      ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
      coalesce_interval_ = 0;
    }
    
    // Parameter conversions
    double autorepeat_interval = 1 / autorepeat_rate_;
    double scale = -1. / (1. - deadzone_) / 32767.;
    double unscaled_deadzone = 32767. * deadzone_;

    js_event event;
    struct timeval tv;
    fd_set set;
    int joy_fd;
    event_count_ = 0;
    pub_count_ = 0;
    lastDiagTime_ = ros::Time::now().toSec();
    
    // Big while loop opens, publishes
    while (nh_.ok())
    {                                      
      open_ = false;
      diagnostic_.force_update();
      bool first_fault = true;
      while (true)
      {
        ros::spinOnce();
        if (!nh_.ok())
          goto cleanup;
        joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        if (joy_fd != -1)
        {
          // There seems to be a bug in the driver or something where the
          // initial events that are to define the initial state of the
          // joystick are not the values of the joystick when it was opened
          // but rather the values of the joystick when it was last closed.
          // Opening then closing and opening again is a hack to get more
          // accurate initial state data.
          close(joy_fd);
          joy_fd = open(joy_dev_.c_str(), O_RDONLY);
        }
        if (joy_fd != -1)
          break;
        if (first_fault)
        {
          ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
          first_fault = false;
        }
        sleep(1.0);
        diagnostic_.update();
      }
      
      ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
      open_ = true;
      diagnostic_.force_update();
      
      bool tv_set = false;
//.........这里部分代码省略.........