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

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

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

bool QuadrotorHardwareSim::initSim(
    const std::string& robot_namespace,
    ros::NodeHandle model_nh,
    gazebo::physics::ModelPtr parent_model,
    const urdf::Model *const urdf_model,
    std::vector<transmission_interface::TransmissionInfo> transmissions)
{
  ros::NodeHandle param_nh(model_nh, "controller");

  // store parent model pointer
  model_ = parent_model;
  link_ = model_->GetLink();
  physics_ = model_->GetWorld()->GetPhysicsEngine();

  // subscribe state
  std::string state_topic;
  param_nh.getParam("state_topic", state_topic);
  if (!state_topic.empty()) {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(state_topic, 1, boost::bind(&QuadrotorHardwareSim::stateCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
    subscriber_state_ = model_nh.subscribe(ops);

    gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_state_.getTopic() << "' as state input for control" << std::endl;
  } else {
    gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as state input for control" << std::endl;
  }

  // subscribe imu
  std::string imu_topic;
  param_nh.getParam("imu_topic", imu_topic);
  if (!imu_topic.empty()) {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(imu_topic, 1, boost::bind(&QuadrotorHardwareSim::imuCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
    subscriber_imu_ = model_nh.subscribe(ops);
    gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_imu_.getTopic() << "' as imu input for control" << std::endl;
  } else {
    gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as imu input for control" << std::endl;
  }

  // subscribe motor_status
  {
    ros::SubscribeOptions ops = ros::SubscribeOptions::create<airdrone_gazebo::MotorStatus>("motor_status", 1, boost::bind(&QuadrotorHardwareSim::motorStatusCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
    subscriber_motor_status_ = model_nh.subscribe(ops);
  }

  // publish wrench
  {
    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>("command/wrench", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
    publisher_wrench_command_ = model_nh.advertise(ops);
  }

  // publish motor command
  {
    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<airdrone_gazebo::MotorCommand>("command/motor", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
    publisher_motor_command_ = model_nh.advertise(ops);
  }

  return true;
}

// +++++++++++++++++++++++++ main loop +++++++++++++++++++++++++++++++++++++++++++
void setup()
{
  // init
  nh.initNode();
  // setup
  setupMotor();
  setupSensor();
  setupServo();
  // advertise
  nh.advertise(pub_sensor_tracks);
  nh.advertise(pub_range);
  // subscribe
  nh.subscribe(subscriberServo);
  nh.subscribe(motor_sub);
}

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

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

void setup()
{
  nh.initNode();

  pinMode(led, OUTPUT);
  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.advertise(ps_voltage);
  Serial.begin(57600);
}

ros::Publisher ShapeShifter::advertise(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size_, bool latch, const ros::SubscriberStatusCallback &connect_cb) const
{
    ros::AdvertiseOptions opts(topic, queue_size_, getMD5Sum(), getDataType(), getMessageDefinition(), connect_cb);
    opts.latch = latch;

    return nh.advertise(opts);
}

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

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

    }

}

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();
  nh.advertise(pause_pub);
  nh.subscribe(rate_sub);

  pinMode(PAUSE_PIN, INPUT);
  digitalWrite(PAUSE_PIN, HIGH);//enable pullup
  pinMode(LIGHT_PIN, OUTPUT);
  digitalWrite(LIGHT_PIN, LIGHT_ON);
}

void setup()
{

  motor_VFL.attach(MOTOR_VFL_PIN);
  motor_VFR.attach(MOTOR_VFR_PIN);
  motor_VBL.attach(MOTOR_VBL_PIN);
  motor_VBR.attach(MOTOR_VBR_PIN);
  motor_HFL.attach(MOTOR_HFL_PIN);
  motor_HFR.attach(MOTOR_HFR_PIN);
  motor_HBL.attach(MOTOR_HBL_PIN);
  motor_HBR.attach(MOTOR_HBR_PIN);


  motor_VFL.writeMicroseconds(STOP_PWM);
  motor_VFR.writeMicroseconds(STOP_PWM);
  motor_VBL.writeMicroseconds(STOP_PWM);
  motor_VBR.writeMicroseconds(STOP_PWM);
  motor_HFL.writeMicroseconds(STOP_PWM);
  motor_HFR.writeMicroseconds(STOP_PWM);
  motor_HBL.writeMicroseconds(STOP_PWM);
  motor_HBR.writeMicroseconds(STOP_PWM);
  delay(1000);


  Wire.begin();
  sDepth.init();
  sDepth.setFluidDensity(997);

  pinMode(START_IN_PIN, INPUT_PULLUP);
  pinMode(STOP_IN_PIN, INPUT_PULLUP);

  nh.initNode();
  nh.advertiseService(server2);
  nh.advertise(pDepth);
  nh.advertise(pStart);
  nh.advertise(pStop);
  nh.advertise(chatter);
  nh.subscribe(sub);

}