C++ NodeHandle::createTimer方法代码示例

  VrpnClientRos::VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh)
  {
    output_nh_ = private_nh;

    host_ = getHostStringFromParams(private_nh);

    ROS_INFO_STREAM("Connecting to VRPN server at " << host_);
    connection_ = std::shared_ptr<vrpn_Connection>(vrpn_get_connection_by_name(host_.c_str()));
    ROS_INFO("Connection established");

    double update_frequency;
    private_nh.param<double>("update_frequency", update_frequency, 100.0);
    mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency), boost::bind(&VrpnClientRos::mainloop, this));

    double refresh_tracker_frequency;
    private_nh.param<double>("refresh_tracker_frequency", refresh_tracker_frequency, 0.0);

    if (refresh_tracker_frequency > 0.0)
    {
      refresh_tracker_timer_ = nh.createTimer(ros::Duration(1 / refresh_tracker_frequency),
                                              boost::bind(&VrpnClientRos::updateTrackers, this));
    }

    std::vector<std::string> param_tracker_names_;
    if (private_nh.getParam("trackers", param_tracker_names_))
    {
      for (std::vector<std::string>::iterator it = param_tracker_names_.begin();
           it != param_tracker_names_.end(); ++it)
      {
        trackers_.insert(std::make_pair(*it, std::make_shared<VrpnTrackerRos>(*it, connection_, output_nh_)));
      }
    }
  }

 // If artificial switching, this method is called at set intervals (according to delTon, delToff) to toggle the estimator
 void switchingTimerCB(const ros::TimerEvent& event)
 {
     if (estimatorOn)
     {
         estimatorOn = false;
         switchingTimer = nh.createTimer(ros::Duration(delToff),&EKF::switchingTimerCB,this,true);
     }
     else
     {
         estimatorOn = true;
         switchingTimer = nh.createTimer(ros::Duration(delTon),&EKF::switchingTimerCB,this,true);
     }
 }

    FollowerFSM() : hold(true),
        currentState(STOPPED),
        nextState(STOPPED),
        nPriv("~")


    {
        //hold = true;
        //currentState = STOPPED;
        //nextState = STOPPED;

        nPriv.param<std::string>("world_frame", world_frame, "world_lol");
        nPriv.param<std::string>("robot_frame", robot_frame, "robot");
        nPriv.param<std::string>("fixed_frame", fixed_frame_id, "odom");
        nPriv.param<double>("minimum_distance", minimum_distance, 2);
        nPriv.param<double>("planner_activation_distance", planner_activation_distance, 1.7);
        nPriv.param("minimum_planner_distance", minimum_planner_distance, 1.7);
        nPriv.param("distance_to_target", distance_to_target, 1.5);
        nPriv.param<double>("kv", kv, 0.03);
        nPriv.param<double>("kalfa", kalfa, 0.08);
        //nPriv.param<double>("rate", frequency, 10);
        rate = new ros::Rate(10.0);

        nPriv.param<std::string>("control_type", cType, "planner");

        if(cType == "planner")
        { ac = new MoveBaseClient("move_base", true);}

        notReceivedNumber = 0;
        sub = n.subscribe("person_position", 1, &FollowerFSM::receiveInformation, this);
        cmdPub = n.advertise<geometry_msgs::Twist>("twist_topic", 1);

        timer = n.createTimer(ros::Duration(0.1), &FollowerFSM::checkInfo, this); //If we didn't receive any position on 2 seconds STOP the robot!
        timer.start();
    }

EventsGenerator::EventsGenerator(ros::NodeHandle& n, ros::NodeHandle& np)
{
	double p;
	np.param("exp_timeout", p, 0.5);
	exp_timeout_ = ros::Duration(p);

	sub_desires_ = n.subscribe("desires_set", 1,
			&EventsGenerator::desiresCB, this);
	sub_intention_ = n.subscribe("intention", 1,
			&EventsGenerator::intentionCB, this);
	srv_cem_ = n.advertiseService("create_exploitation_matcher",
			&EventsGenerator::cemCB, this);
	srv_ctem_ = n.advertiseService("create_topic_exploitation_matcher",
			&EventsGenerator::ctemCB, this);

	pub_events_ = n.advertise<hbba_msgs::Event>("events", 100);

	ros::NodeHandle rosgraph_np(np, "rosgraph_monitor");
	rosgraph_monitor_.reset(new RosgraphMonitor(n, rosgraph_np));
	rosgraph_monitor_->registerCB(&EventsGenerator::rosgraphEventsCB, this);

	timer_ = n.createTimer(ros::Duration(1.0), &EventsGenerator::timerCB, this);

    parseExploitationMatches(n);
}

        AngularRateControl() : nh_("~"),lastIMU_(0.0),
        lastCommand_(0.0), timeout_(1.5),
        joy_control_("/teleop/twistCommand"), auto_control_("/mux/safeCommand"),
        manual_override_(false), rc_override_(true)
         {
            nh_.param("kp",kp_,1.0);
            nh_.param("ki",ki_,0.0);
            nh_.param("kd",kd_,0.0);
            nh_.param("imax",imax_,1E9);
            nh_.param("period",period_,0.05);
            nh_.param("motor_zero",motor_zero_,0.0);
            nh_.param("input_scale",input_scale_,1.0);
            double dTimeout;
            nh_.param("timeout",dTimeout,1.5);
            timeout_ = ros::Duration(dTimeout);

            Pid_.initPid(kp_,ki_,kd_,imax_,-imax_);

            // Make sure TF is ready
            ros::Duration(0.5).sleep();

            // Subscribe to the velocity command and setup the motor publisher
            imu_sub_ = nh_.subscribe("imu",1,&AngularRateControl::imu_callback,this);
            cmd_sub_ = nh_.subscribe("command",1,&AngularRateControl::cmd_callback,this);
            mux_sub_ = nh_.subscribe("/mux/selected",1,&AngularRateControl::mux_callback,this);
            rc_sub_ = nh_.subscribe("/sense",1,&AngularRateControl::rc_callback,this);
            motor_pub_ = nh_.advertise<geometry_msgs::Twist>("motors",1);
            debug_pub_ = nh_.advertise<geometry_msgs::Point>("debug",1);
            //Create a callback for the PID loop
            pid_loop_ = nh_.createTimer(ros::Duration(period_), &AngularRateControl::do_pid, this);

            f_ = boost::bind(&AngularRateControl::config_callback, this, _1, _2);
            server_.setCallback(f_);

        }

Experiment::Experiment(ros::NodeHandle n):n_(n)
{
	ros::NodeHandle n_private("~");
	n_private.param("num_reps", num_reps_, 1000);
	n_private.param("freq", freq_, 1.0);
	n_private.param("start_x", start_x_, -5.0);
	n_private.param("start_y", start_y_, 5.0);
	n_private.param("goal_radius", radius_, 0.5);
	n_private.param("learn", learn_, true);
	n_private.param("learning_rate", learning_rate_, 0.5);
	n_private.param("discount_factor", discount_factor_, 0.5);
	n_private.param("max_explore", max_explore_, 8);

	cnt_rep_ = 0;
	move_stopped_ = false;
	mode_ = MODE_REP_START;

	states_ = new States(n);
	actions_ = new Actions(n);
	qobj_ = new QLearner(actions_->GetNumActions(),
			states_->GetNumStates(), learning_rate_, discount_factor_, learn_,
			max_explore_);

	bool_sub_ = n.subscribe("/move_done", 1, &Experiment::bool_cb, this);
//	ros::Subscriber odom_sub_ = n.subscribe("/odom", 10, odom_cb);
	client_ = n.serviceClient<stage_light_ml::move_robot>("/move_robot");
	timer_ = n.createTimer(ros::Duration(1/freq_), &Experiment::timer_cb, this);
}

	GenericDiagnostic(std::string diagnosticName):nh() {
		_updater.add(diagnosticName,this, &GenericDiagnostic::nodeDiagnostics);
		_timer = nh.createTimer(ros::Duration(4),&GenericDiagnostic::diagnosticTimerCallback,this);
		_updater.setHardwareID("none");
		
		
	};

  Node()
      : private_nh("~"),
        kill_listener(boost::bind(&Node::killed_callback, this)),
        actionserver(nh, "moveto", false),
        disabled(false) {
    fixed_frame = uf_common::getParam<std::string>(private_nh, "fixed_frame");
    body_frame = uf_common::getParam<std::string>(private_nh, "body_frame");

    limits.vmin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmin_b");
    limits.vmax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "vmax_b");
    limits.amin_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amin_b");
    limits.amax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "amax_b");
    limits.arevoffset_b = uf_common::getParam<Eigen::Vector3d>(private_nh, "arevoffset_b");
    limits.umax_b = uf_common::getParam<subjugator::Vector6d>(private_nh, "umax_b");
    traj_dt = uf_common::getParam<ros::Duration>(private_nh, "traj_dt", ros::Duration(0.0001));

    odom_sub = nh.subscribe<Odometry>("odom", 1, boost::bind(&Node::odom_callback, this, _1));

    trajectory_pub = nh.advertise<PoseTwistStamped>("trajectory", 1);
    waypoint_pose_pub = private_nh.advertise<PoseStamped>("waypoint", 1);

    update_timer =
        nh.createTimer(ros::Duration(1. / 50), boost::bind(&Node::timer_callback, this, _1));

    actionserver.start();

    set_disabled_service = private_nh.advertiseService<SetDisabledRequest, SetDisabledResponse>(
        "set_disabled", boost::bind(&Node::set_disabled, this, _1, _2));
  }

WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) :
    nh_(nh), image_transport_(nh),
    handler_group_(http_server::HttpReply::stock_reply(http_server::HttpReply::not_found))
{
  cleanup_timer_ = nh.createTimer(ros::Duration(0.5), boost::bind(&WebVideoServer::cleanup_inactive_streams, this));

  int port;
  private_nh.param("port", port, 8080);

  int server_threads;
  private_nh.param("server_threads", server_threads, 1);

  private_nh.param("ros_threads", ros_threads_, 2);

  stream_types_["mjpeg"] = boost::shared_ptr<ImageStreamerType>(new MjpegStreamerType());
  stream_types_["vp8"] = boost::shared_ptr<ImageStreamerType>(new LibavStreamerType("webm", "libvpx", "video/webm"));

  handler_group_.addHandlerForPath("/", boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2));
  handler_group_.addHandlerForPath("/stream", boost::bind(&WebVideoServer::handle_stream, this, _1, _2));
  handler_group_.addHandlerForPath("/stream_viewer", boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2));
  handler_group_.addHandlerForPath("/snapshot", boost::bind(&WebVideoServer::handle_snapshot, this, _1, _2));

  server_.reset(new http_server::HttpServer("0.0.0.0", boost::lexical_cast<std::string>(port),
      boost::bind(ros_connection_logger, handler_group_, _1, _2), server_threads));
}

		/*!
		* \brief Initializes node to get parameters, subscribe and publish to topics.
		*/
		bool init()
		{
			// implementation of topics to publish
 
			nh_.param("dsadevicestring", dsadevicestring_, std::string(""));
			if (dsadevicestring_.empty()) return false;

			nh_.param("dsadevicenum", dsadevicenum_, 0);
			nh_.param("maxerror", maxerror_, 8);
			
 			double publish_frequency, diag_frequency;
			
			nh_.param("debug", debug_, false);
			nh_.param("polling", polling_, false);
			nh_.param("use_rle", use_rle_, true);
			nh_.param("diag_frequency", diag_frequency, 5.0);
			frequency_ = 30.0;
			if(polling_) nh_.param("poll_frequency", frequency_, 5.0);
			nh_.param("publish_frequency", publish_frequency, 0.0);
			
			auto_publish_ = true;

				
			if(polling_){
			    timer_dsa = nh_.createTimer(ros::Rate(frequency_).expectedCycleTime(),boost::bind(&DsaNode::pollDsa,  this));
 			}else{
			    timer_dsa = nh_.createTimer(ros::Rate(frequency_*2.0).expectedCycleTime(),boost::bind(&DsaNode::readDsaFrame,  this));
			    if(publish_frequency > 0.0){
				auto_publish_ = false;
				timer_publish = nh_.createTimer(ros::Rate(publish_frequency).expectedCycleTime(),boost::bind(&DsaNode::publishTactileData, this));
			    }
			}

			timer_diag = nh_.createTimer(ros::Rate(diag_frequency).expectedCycleTime(),boost::bind(&DsaNode::publishDiagnostics, this));
			
			if(!read_vector(nh_, "dsa_reorder", dsa_reorder_)){
			    dsa_reorder_.resize(6);
			    dsa_reorder_[0] = 2; // t1
			    dsa_reorder_[1] = 3; // t2
			    dsa_reorder_[2] = 4; // f11
			    dsa_reorder_[3] = 5; // f12
			    dsa_reorder_[4] = 0; // f21
			    dsa_reorder_[5] = 1; // f22
			}
			
			return true;
		}

  /****************************************************
   * @brief : Default constructor
   * @param : ros node handler
   ****************************************************/
  RobotObserver(ros::NodeHandle& node)
  {
    node_=node;
    
    // Getting robot's id from ros param
    if(node_.hasParam("my_robot_id"))
    {
      node_.getParam("my_robot_id",my_id_);
    } else {
      my_id_="PR2_ROBOT";
    }
    waiting_timer_ = node_.createTimer(ros::Duration(1.0), &RobotObserver::timerCallback, this, true);
    timer_on_=false;
    enable_event_=true;
    task_started_=false;
    // Advertise subscribers
    fact_list_sub_ = node_.subscribe("/agent_monitor/factList", 1, &RobotObserver::factListCallback, this);
    fact_area_list_sub_ = node_.subscribe("/area_manager/factList", 1, &RobotObserver::factListAreaCallback, this);
    human_list_sub_ = node_.subscribe("/pdg/humanList", 1, &RobotObserver::humanListCallback, this);
    object_list_sub_ = node_.subscribe("/pdg/objectList", 1, &RobotObserver::objectListCallback, this);
    current_action_list_sub_ = node_.subscribe("/human_monitor/current_humans_action", 1, &RobotObserver::CurrentActionListCallback, this);
    next_action_list_sub_ = node_.subscribe("/plan_executor/next_humans_actions", 1, &RobotObserver::NextActionListCallback, this);
    // Advertise publishers
    attention_vizu_pub_ = node_.advertise<geometry_msgs::PointStamped>("head_manager/attention_vizualisation", 5);

    init_action_client_ = new InitActionClient_t("pr2motion/Init", true);
    // Initialize action client for the action interface to the head controller
    head_action_client_ = new HeadActionClient_t("pr2motion/Head_Move_Target", true);
    // Connection to the pr2motion client
    connect_port_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/connect_port");
    head_stop_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/Head_Stop");
    ROS_INFO("[robot_observer] Waiting for pr2motion action server to start.");
    init_action_client_->waitForServer(); //will wait for infinite time
    head_action_client_->waitForServer(); //will wait for infinite time
    ROS_INFO("[robot_observer] pr2motion action server started.");

    pr2motion::InitGoal goal_init;
    init_action_client_->sendGoal(goal_init);

    pr2motion::connect_port srv;
    srv.request.local = "joint_state";
    srv.request.remote = "joint_states";
    if (!connect_port_srv_.call(srv)){
      ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
    }
    srv.request.local = "head_controller_state";
    srv.request.remote = "/head_traj_controller/state";
    if (!connect_port_srv_.call(srv)){
      ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
    }
    pr2motion::Z_Head_SetMinDuration srv_MinDuration;
    srv_MinDuration.request.head_min_duration=0.6;
    if(!ros::service::call("/pr2motion/Z_Head_SetMinDuration",srv_MinDuration))
        ROS_ERROR("[robot_observer] Failed to call service /pr2motion/Z_Head_SetMinDuration");
    state_machine_ = new ObserverStateMachine(boost::cref(this));
    state_machine_->start();
    ROS_INFO("[robot_observer] Starting state machine, node ready !");
  }

void PlayerTracker::spin()
{
	ros::Timer timer = node.createTimer(ros::Duration(1.0/RATE), &PlayerTracker::updateOdometry,this);
	ros::Rate r(RATE);
	while(ros::ok()) {
		ros::spinOnce();
		r.sleep();
	}
}

CJointController::CJointController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh, std::map<std::string,CServo *>& servos) : CController(name,device_p,nh)
{
    joints_dirty_=true;
    servos_p_=&servos;
    std::string topic;
    nh.param("controllers/"+name+"/topic", topic, std::string("cmd_joints"));
    nh.param("controllers/"+name+"/rate", rate_, 15.0);
    joint_sub_ = nh.subscribe<sensor_msgs::JointState>(topic, 10, &CJointController::jointCallback, this);
    timer_=nh.createTimer(ros::Duration(1/rate_),&CJointController::timerCallback,this);
}

    KeyboardNode() {
	ROS_INFO("Starting Keyboard Node...");
	// wait for the simulator reset service to become available:
	ROS_DEBUG("Waiting for simulator_reset service to become available");
	ros::service::waitForService("simulator_reset");
	// create a service client
	reset_client = n_.serviceClient<std_srvs::Empty>("simulator_reset");
	timer = n_.createTimer(ros::Duration(0.02),
			       &KeyboardNode::timerCallback, this);
    }

CBatteryController::CBatteryController(std::string name, CQboduinoDriver *device_p, ros::NodeHandle& nh) : CController(name,device_p,nh)
{
    level_=0;
    stat_=0;
    std::string topic;
    nh.param("controllers/"+name+"/topic", topic, std::string("battery_state"));
    nh.param("controllers/"+name+"/rate", rate_, 15.0);
    battery_pub_ = nh.advertise<qbo_arduqbo::BatteryLevel>(topic, 1);
    timer_=nh.createTimer(ros::Duration(1/rate_),&CBatteryController::timerCallback,this);
}