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

int main()
{
  int error = 0;
  t.start();

  nh.initNode();
  nh.advertise(pub_temp);
  nh.advertise(pub_humidity);

  long publisher_timer = 0;
  temp_msg.header.frame_id = "/base_link";
  humidity_msg.header.frame_id = "/base_link";

  while (1)
  {

    if (t.read_ms() > publisher_timer)
    {
      error = sensor.readData();
      if (0 == error)
      {
        temp_msg.temperature = sensor.ReadTemperature(CELCIUS);
        temp_msg.header.stamp = nh.now();
        pub_temp.publish(&temp_msg);

        humidity_msg.relative_humidity = sensor.ReadHumidity();
        humidity_msg.header.stamp = nh.now();
        pub_humidity.publish(&humidity_msg);
      }
      publisher_timer = t.read_ms() + 1000;
    }
    nh.spinOnce();
  }
}

int main(void)
{
  // TivaC application specific code
  MAP_FPUEnable();
  MAP_FPULazyStackingEnable();
  // TivaC system clock configuration. Set to 80MHz.
  MAP_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);

  uint8_t button_debounced_delta;
  uint8_t button_raw_state;
  ButtonsInit();

  // ROS nodehandle initialization and topic registration
  nh.initNode();
  nh.advertise(button_publisher);

  while (1)
  {
    uint8_t button_debounced_state = ButtonsPoll(&button_debounced_delta, &button_raw_state);
    // Publish message to be transmitted.
    button_msg.sw1.data = button_debounced_state & LEFT_BUTTON;
    button_msg.sw2.data = button_debounced_state & RIGHT_BUTTON;
    button_publisher.publish(&button_msg);

    // Handle all communications and callbacks.
    nh.spinOnce();

    // Delay for a bit.
    nh.getHardware()->delay(100);
  }
}

// Note: connector labeled "INPUT" on sonar sensor goes to
// digital 1 (bit 0), and connector labeled "OUTPUT" goes to
// digital 2 (bit 1).
int main()
{
  CQEGpioInt &gpio = CQEGpioInt::GetRef();
  volatile unsigned int d;

  // reset bit 0, set as output for sonar trigger
  gpio.SetData(0x0000);
  gpio.SetDataDirection(0x0001);

  // set callbacks on negative edge for both bits 0 (trigger)
  // and 1 (echo)
  gpio.RegisterCallback(0, NULL, callback);
  gpio.RegisterCallback(1, NULL, callback);
  gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
  gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);

	//nh.initNode();
	nh.initNode(rosSrvrIp);
	nh.advertise(sonar1);

  // trigger sonar by toggling bit 0
  while(1)
    {
      gpio.SetData(0x0001);
      for (d=0; d<120000; d++);
      gpio.SetData(0x0000);
      sleep(1);		// the interrupt breaks us out of this sleep
      sleep(1);		// now really sleep a second
	  sonar1.publish( &range );
	  nh.spinOnce();

    }

}

int main() {
   unsigned long last_pub;

   /* set up interrupt handling */
   // set up timer interrupts
   // fast PWM mode; interrupt and reset when counter equals OCR0A
   // prescalar 64
   TCCR0A = (1 << WGM01 | 1 << WGM00);
   TCCR0B = (1 << WGM02 | 1 << CS01 | 1 << CS00);
   // interrupt on "overflow" (counter match)
   TIMSK0 = (1 << TOIE0);
   OCR0A  = 249; // 250 counts per tick


   nh.initNode();
   nh.advertise(pub);

   nh.subscribe(sub);

   last_pub = nh.now().toNsec();

   while(1) {
      nh.spinOnce();
      // do our best to publish once per second
      if( nh.now().toNsec() - last_pub > 1000000000ull ) {
         pub.publish(&msg);
         last_pub += 1000000000ull;
      }
   }
}

int main() {
    t.start();

    nh.initNode();
    nh.advertise(pub_temp);

    long publisher_timer =0;

    while (1) {

        if (t.read_ms() > publisher_timer) {
            // step 1: request reading from sensor
            //Wire.requestFrom(sensorAddress,2);
            char cmd = 2;
            i2c.write(sensorAddress, &cmd, 1);

            wait_ms(50);

            char msb;
            char lsb;
            int temperature;
            i2c.read(sensorAddress, &msb, 1); // receive high byte (full degrees)
            i2c.read(sensorAddress, &lsb, 1); // receive low byte (fraction degrees)

            temperature = ((msb) << 4);  // MSB
            temperature |= (lsb >> 4);   // LSB

            temp_msg.data = temperature*0.0625;
            pub_temp.publish(&temp_msg);

            publisher_timer = t.read_ms() + 1000;
        }

        nh.spinOnce();
    }

/**
 * @brief Initializes the gyro/accelerometer and the magnetometer unit of the imu.
 * 		As well as the arduino subsystem
 */
void setup() {
    Wire.begin();
    delay(1500);

    /********/
    /* GYRO */
    /********/
    gyro.init();
    gyro.writeReg(L3G_CTRL_REG4, 0x00); // 245 dps scale
    gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
    //8.75 mdps/LSB

    /****************/
    /* MAGNETOMETER */
    /****************/
    compass.init();
    compass.enableDefault();
    compass.writeReg(LSM303::CTRL2, 0x08); // 4 g scale: AFS = 001
    //0.122 mg/LSB
    compass.writeReg(LSM303::CTRL5, 0x10); // Magnetometer Low Resolution 50 Hz
    //Magnetometer 4 gauss scale : 0.16mgauss/LSB


    //ROS-TF base frame of the imu_data
    imu_msg.header.frame_id="base_imu_link";

    //Register ROS messages
    nh.initNode();
    nh.advertise(imu_pub);
    nh.advertise(mag_pub);

    //starting value for the timer
    timer=millis();
}

void setup()
{
  nh.initNode();

  pinMode(directionPinEast1, OUTPUT);
  pinMode(directionPinEast2, OUTPUT);
  pinMode(pwmPinWest, OUTPUT);
  pinMode(directionPinWest2, OUTPUT);
  pinMode(pwmPinEast, OUTPUT);
  pinMode(directionPinWest1, OUTPUT);

  pinMode(directionPinSouthSway1, OUTPUT);
  pinMode(directionPinSouthSway2, OUTPUT);
  pinMode(pwmPinNorthSway, OUTPUT);
  pinMode(directionPinNorthSway2, OUTPUT);
  pinMode(pwmPinSouthSway, OUTPUT);
  pinMode(directionPinNorthSway1, OUTPUT);

  pinMode(directionPinSouthUp1, OUTPUT);
  pinMode(directionPinSouthUp2, OUTPUT);
  pinMode(pwmPinNorthUp, OUTPUT);
  pinMode(directionPinNorthUp2, OUTPUT);
  pinMode(pwmPinSouthUp, OUTPUT);
  pinMode(directionPinNorthUp1, OUTPUT);

  nh.subscribe(subPwmForward);
  nh.subscribe(subPwmSideward);
  nh.subscribe(subPwmUpward);
  nh.subscribe(subPwmTurn);
  nh.subscribe(subPwmTurnSway);
  Serial.begin(57600);
}

int main()
{
	volatile unsigned int d;

	/* initialize ROS & subscribers & publishers */
	//nh.initNode();
	nh.initNode(rosSrvrIp);
	nh.advertise(sonar1);	// advertise sonar range topic
	nh.subscribe(motorSub);		// subscribe to motor speed topic
	nh.subscribe(servoSub);		// subscribe to servo position

	// reset bit 0, set as output for sonar trigger
	gpio.SetData(0x0000);
	gpio.SetDataDirection(0x0001);

	// set callbacks on negative edge for both bits 0 (trigger)
	// and 1 (echo)
	gpio.RegisterCallback(0, NULL, callback);
	gpio.RegisterCallback(1, NULL, callback);
	gpio.SetInterruptMode(0, QEG_INTERRUPT_NEGEDGE);
	gpio.SetInterruptMode(1, QEG_INTERRUPT_NEGEDGE);

	// trigger sonar by toggling bit 0
	while(1)
	{
		gpio.SetData(0x0001);
		for (d=0; d<120000; d++);
		gpio.SetData(0x0000);
		usleep(100000);		// the interrupt breaks us out of this sleep
		usleep(100000);		// now really sleep
		sonar1.publish( &range );
		nh.spinOnce();
	}
}

void setup()
{
    nh.initNode();
    nh.advertise(sensor_val);
    nh.advertise(sensor_voltage);
    nh.advertise(sensor_temp);
}

void setup() {
    // put your setup code here, to run once:
    StepMotor.setSpeed(150);
    pinMode(Button, INPUT_PULLUP);
    pinMode(Sensor_Down, INPUT_PULLUP);
    pinMode(Sensor_Up, INPUT_PULLUP);
    pinMode(EnA, OUTPUT);
    pinMode(EnB, OUTPUT);
    nh.initNode();
    nh.subscribe(sub);
    nh.advertise(answer_pub);
    nh.advertise(switch_pub);
    if(digitalRead(Button)){
        deadman_activated = true;
        deadman_switch_.data = deadman_activated;
        deadman_switch_.header.stamp = nh.now();
        switch_pub.publish(&deadman_switch_);
    }
    else{
        deadman_activated = false;
        deadman_switch_.data = deadman_activated;
        deadman_switch_.header.stamp = nh.now();
        switch_pub.publish(&deadman_switch_);
    }

    down();
}

void setup() {
	nh.initNode();
	//Serial.begin(38400);
	delay(1000);

	rlog = new RosLogger(nh);
//	quadratureEncoder = new QuadratureEncoder();
//	lineSensor = new LineSensor();
//	lineSensor->calibrate();
	motor = new Motor(nh);
	Motor::Command c;
	//c.direction = Motor::STOP; motor->enqueue(c);
	c.direction = Motor::BACKWARD; motor->enqueue(c);
	c.direction = Motor::FORWARD; motor->enqueue(c);
	// c.direction = Motor::STOP; motor->enqueue(c);
	// c.direction = Motor::RIGHT_TURN; motor->enqueue(c);
	// c.direction = Motor::STOP; motor->enqueue(c);
	// c.direction = Motor::BACKWARD; motor->enqueue(c);
	// c.direction = Motor::STOP; motor->enqueue(c);
	// c.direction = Motor::LEFT_TURN; motor->enqueue(c);
	// c.direction = Motor::STOP; motor->enqueue(c);
	for (int i = 0; i < 2; i++) {
		nh.spinOnce();
		delay(1000);
	}
	
	for (int i = 0; i < 4; i++) {
		rlog->info("START UP %d", i);
		nh.spinOnce();
		delay(200);
	}
}

void setup() {
    pinMode(13, OUTPUT);

    nh.initNode();
    nh.subscribe(sub);

    servo.attach(9); //attach it to pin 9
}

 void init()
 {
   nh.initNode();
   //~ nh.subscribe(sub);
   nh.advertiseService(fireService);
   nh.advertiseService(cancelService);
   nh.advertiseService(manualService);
 }

int main()
{
  // SET single LED port
  DDRC |= _BV(LED_PIN);
  PORTC ^= _BV(LED_PIN);
  // 1s pull up of LED Strip pins */
  apa102_DDRREG &= ~_BV(apa102_data);
  apa102_DDRREG &= ~_BV(apa102_clk);
  apa102_PORTREG |= _BV(apa102_data);
  apa102_PORTREG |= _BV(apa102_clk);
  _delay_ms(900);
  // Disable pull ups
  apa102_PORTREG &= ~_BV(apa102_data);
  apa102_PORTREG &= ~_BV(apa102_clk);
  apa102_DDRREG |= _BV(apa102_data);
  apa102_DDRREG |= _BV(apa102_clk);
  _delay_ms(100);
  PORTC ^= _BV(LED_PIN);

  // Clear LEDs in the strip
  clear_all_leds();

  // Init ROS
  nh.initNode();
  nh.subscribe(set_sub);
  nh.subscribe(set_led_sub);

  ack_led();

  // Wait for Server side to start
  while (!nh.connected())
  {
    nh.spinOnce();
    // LUFA functions that need to be called frequently to keep USB alive
    CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
    USB_USBTask();
    _delay_ms(10);
  }

  ack_led();

  // Publish some debug information
  snprintf(log_str, MAX_MSG_SIZE, "V:%s", GIT_VERSION);
  nh.loginfo(log_str);
  snprintf(log_str, MAX_MSG_SIZE, "FM:%d", get_free_ram());
  nh.loginfo(log_str);

  while(1)
  {
    nh.spinOnce();
    // LUFA functions that need to be called frequently to keep USB alive
    CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
    USB_USBTask();
  }

  return 0;
}

void setup() 
{
  RelayShield::begin();

  node_handle.getHardware()->setBaud(115200);
  node_handle.initNode();

  node_handle.subscribe(relay_subscriber);
}