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

/**
 * @brief provides imu readings in a 50 Hz rate.
 *
 */
void loop() {
    if((millis()-timer)>=20) { // Main loop runs at 50Hz
        timer=millis();

        //Read data from the hardware

        gyro.read();
        compass.readAcc();
        compass.readMag();

        //Assign read data to the ros messages

        imu_msg.angular_velocity.x=gyro.g.x;
        imu_msg.angular_velocity.y=gyro.g.y;
        imu_msg.angular_velocity.z=gyro.g.z;

        imu_msg.linear_acceleration.x=compass.a.x;
        imu_msg.linear_acceleration.y=compass.a.y;
        imu_msg.linear_acceleration.z=compass.a.z;

        mag_msg.magnetic_field.x=compass.m.x;
        mag_msg.magnetic_field.y=compass.m.y;
        mag_msg.magnetic_field.z=compass.m.z;

        //Publish the data to the ros message system

        imu_pub.publish( &imu_msg );
        mag_pub.publish( &mag_msg);
        nh.spinOnce();
    }
    nh.spinOnce();
}

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);
	}
}

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;
}

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();
    }

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();
	}
}

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);
  }
}

void loop(){
  unsigned long pulse_length = pulseIn(PAUSE_PIN, HIGH);
  bool_msg.data = pulse_length>1400;//will not pause if pulseIn timed out (no pulse = 0)
  pause_pub.publish( &bool_msg );

  //if(nh.connected()){
   if( !bool_msg.data)
{
      unsigned long time = millis();
      if(time-last_invert>=(1000/1/2)){
	digitalWrite(LIGHT_PIN, !digitalRead(LIGHT_PIN));
	last_invert = time;
    }
}

  //  }
    else
{
      digitalWrite(LIGHT_PIN, LIGHT_ON);
  }
 // else
 //   digitalWrite(LIGHT_PIN, LIGHT_ON);
  

  delay(50);

  nh.spinOnce();
}

// 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;
      }
   }
}

void loop()
{

  motor_VFL.writeMicroseconds(MotorPWM[MOTOR_VFL-1]);
  motor_VFR.writeMicroseconds(MotorPWM[MOTOR_VFR-1]);
  motor_VBL.writeMicroseconds(MotorPWM[MOTOR_VBL-1]);
  motor_VBR.writeMicroseconds(MotorPWM[MOTOR_VBR-1]);
  motor_HFL.writeMicroseconds(MotorPWM[MOTOR_HFL-1]);
  motor_HFR.writeMicroseconds(MotorPWM[MOTOR_HFR-1]);
  motor_HBL.writeMicroseconds(MotorPWM[MOTOR_HBL-1]);
  motor_HBR.writeMicroseconds(MotorPWM[MOTOR_HBR-1]);
  sDepth.read();
  fDepth.data = -1.0 * sDepth.depth();//Mult by -1 so negative depth is down
  pDepth.publish(&fDepth);
  chatter.publish(&str_msg);

  if (digitalRead(START_IN_PIN) == LOW){
    bStart.data = true;
  } else {
    bStart.data = false;
  }

  if (digitalRead(STOP_IN_PIN) == LOW){
    bStop.data = true;
  } else {
    bStop.data = false;
  }

  pStart.publish(&bStart);
  pStop.publish(&bStop);


  nh.spinOnce();

}

void loop()
{
  v = analogRead(analogPinPressureSensor);
  voltage.data = v;
  ps_voltage.publish(&voltage);
  nh.spinOnce();
  delay(1);
}

void loop()
{
  
  // send serial packet: 
  pub.publish( &serial_msg );

  
  nh.spinOnce();
}

void loop()
{
  joint_1_position = jointGet(joint_1_address, I2C_COMMAND_JOINT_GET_POSITION);

  // Report it to the publisher.
  joint_1_position_msg.data = joint_1_position;
  joint_1_position_publisher.publish(&joint_1_position_msg);

  // Now, check if we have a new setpoint from the listener.
  if (joint_1_new_setpoint != joint_1_current_setpoint)
  {
    jointSet(joint_1_address,
	     I2C_COMMAND_JOINT_SET_SETPOINT,
	     joint_1_new_setpoint);
    joint_1_current_setpoint = joint_1_new_setpoint;
  }



  joint_2_position = jointGet(joint_2_address, I2C_COMMAND_JOINT_GET_POSITION);

  // Report it to the publisher.
  joint_2_position_msg.data = joint_2_position;
  joint_2_position_publisher.publish(&joint_2_position_msg);

  // Now, check if we have a new setpoint from the listener.
  if (joint_2_new_setpoint != joint_2_current_setpoint)
  {
    jointSet(joint_2_address,
	     I2C_COMMAND_JOINT_SET_SETPOINT,
	     joint_2_new_setpoint);
    joint_2_current_setpoint = joint_2_new_setpoint;
  }


  joint_3_position = jointGet(joint_3_address, I2C_COMMAND_JOINT_GET_POSITION);

  // Report it to the publisher.
  joint_3_position_msg.data = joint_3_position;
  joint_3_position_publisher.publish(&joint_3_position_msg);

  // Now, check if we have a new setpoint from the listener.
  if (joint_3_new_setpoint != joint_3_current_setpoint)
  {
    jointSet(joint_3_address,
	     I2C_COMMAND_JOINT_SET_SETPOINT,
	     joint_3_new_setpoint);
    joint_3_current_setpoint = joint_3_new_setpoint;
  }



  nh.spinOnce();

  delay(100); 
}

int main() {

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

    while (1) {
        nh.spinOnce();
        wait_ms(1);
    }
}