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

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

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 loop() {
    
    if(digitalRead(Button) && deadman_activated == false){
        deadman_activated = true;
        deadman_switch_.data = deadman_activated;
        deadman_switch_.header.stamp = nh.now();
        switch_pub.publish(&deadman_switch_);
    }

    else if(!digitalRead(Button) && deadman_activated == true){
        deadman_activated = false;
        deadman_switch_.data = deadman_activated;
        deadman_switch_.header.stamp = nh.now();
        switch_pub.publish(&deadman_switch_);
    }

    nh.spinOnce();
    delay(50);
}

void setup() {
	nh.initNode();
	last_time = nh.now();
	current_time = nh.now();

	//Serial.begin(38400);
	//#####delay(1000);

	rlog = new RosLogger(nh);
	//#####quadratureEncoder = new QuadratureEncoder();
//	lineSensor = new LineSensor();
//	lineSensor->calibrate();

	nh.advertise(odom_pub);
	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);
	*/

	nh.subscribe(Motor::sub);

	while (!nh.connected()) { nh.spinOnce(); }
	
	rlog->info("START UP...");
}

void loop()
{
  
  //publish the adc value every 50 milliseconds
  //since it takes that long for the sensor to stablize
  if ( millis() >= range_time ){
    int r =0;

    range_msg.range = getRange_Ultrasound(5);
    range_msg.header.stamp = nh.now();
    pub_range.publish(&range_msg);
    range_time =  millis() + 50;
  }
  
  nh.spinOnce();
}

void down(){
    digitalWrite(EnA, HIGH);
    digitalWrite(EnB, HIGH);

    StepMotor.setSpeed(150);

    if(Sensor_Down!=1){
      while(digitalRead(Sensor_Down)!=1){
        StepMotor.step(-stepsPerRevolution/4);
      }
    }

    digitalWrite(EnA, LOW);
    digitalWrite(EnB, LOW);

    state_ = 2;
    answer_.data = state_;
    answer_.header.stamp = nh.now();
    answer_pub.publish(&answer_);
}

void loopUltra()
{
  // establish variables for duration of the ping,
  // and the distance result in inches and centimeters:
  long duration, inches, cm;

  if (intervalStarted) {
    // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
    // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    pinMode(sonarPin, OUTPUT);
    digitalWrite(sonarPin, LOW);
    delayMicroseconds(2);
    digitalWrite(sonarPin, HIGH);
    delayMicroseconds(15);
    digitalWrite(sonarPin, LOW);
    delayMicroseconds(20);
    // The same pin is used to read the signal from the PING))): a HIGH
    // pulse whose duration is the time (in microseconds) from the sending
    // of the ping to the reception of its echo off of an object.
    pinMode(sonarPin, INPUT);
    duration = pulseIn(sonarPin, HIGH);

    // convert the time into a distance
    cm = microsecondsToCentimeters(duration);

    // put data into message
    range_msg_sonar.radiation_type = sensor_msgs::Range::ULTRASOUND;
    range_msg_sonar.header.frame_id =  "/ultraSonic"; // frame
    range_msg_sonar.header.stamp = nh.now(); // time
    range_msg_sonar.field_of_view = 60.0 / 360 * 3.14; // [rad]
    range_msg_sonar.min_range = 0.03; // [m]
    range_msg_sonar.max_range = 4.0; // [m]
    range_msg_sonar.range = cm / 100.0; // [m]
    // publish
    pub_range.publish(&range_msg_sonar);
  }
}

void loop() {
	nh.spinOnce();
	current_time = nh.now();

	vx = motor->leftVelocity();
	vy = motor->rightVelocity();
	vth = motor->zVelocity();
	double dt = (current_time.toNsec() - last_time.toNsec()) / (1000000000.0);
	double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
	double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
	double delta_th = vth * dt;

	x += delta_x;
	y += delta_y;
	th = delta_th;

	geometry_msgs::Quaternion odom_quat = createQuaternionMsgFromYaw(th);
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;
    //odom_broadcaster.sendTransform(odom_trans);

    nav_msgs::Odometry odom;
    static unsigned long odomSeq = 0;
    odom.header.seq = odomSeq++;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation.x = odom_quat.x;
    odom.pose.pose.orientation.y = odom_quat.y;
    odom.pose.pose.orientation.z = odom_quat.z;
    odom.pose.pose.orientation.w = odom_quat.w;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.linear.z = 0.0;
    odom.twist.twist.angular.x = vth;
    odom.twist.twist.angular.y = vth;
    odom.twist.twist.angular.z = vth;

    for (int i = 0; i < 36; i++) {
    	odom.pose.covariance[i] = 0.0;
    	odom.twist.covariance[i] = 0.0;
    }

    //publish the message
    odom_pub.publish(&odom);

	// lineSensor->read();
	// const LineSensor::TSensorArray& values = lineSensor->sensorValues();

	// sprintf(buffer, "Position: %d, values: L> %d, %d, %d, %d, %d, %d, %d, %d <R",
	// 	lineSensor->position(),
	// 	values[0],
	// 	values[1],
	// 	values[2],
	// 	values[3],
	// 	values[4],
	// 	values[5],
	// 	values[6],
	// 	values[7]
	// 	);
	
	// rlog->info("Number: %d, quad: %d", loopCounter++, QuadratureEncoder::Counter());
	// rlog->info(buffer);
	//rlog->info("Queue len: %d", motor->queueLength());
	//quadratureEncoder->info();
	motor->run();
	last_time = current_time;
	//delay(10);
}

int main(int argc, char* argv[]) 
{  

// These following lines are used to Make sure that command lline args are correct
	

if(argc<5) // if number of args are less than 5
	{
		cerr << "Usage: " << argv[0] << " <socket> <left_motor_port> <right_motor_port> <sensor_port> <hz>" << endl;
		return 1;
	}
	int milliseconds = 100;
	if(argc==6)
    	milliseconds = 1000/atoi(argv[5]);
    // cout<<"milliseconds"<<milliseconds;
    string left_motor_port(argv[2]);
    string right_motor_port(argv[3]);
    string sensor_port(argv[4]);
    // if(left_motor_port<1||left_motor_port>4||right_motor_port<1||right_motor_port>4||left_motor_port==right_motor_port)
    // {
		// cerr << "Invalid motor port numbers. Must be 1, 2, 3 or 4 and distinct." << endl;
		// return 1;
	// }

	// TODO: Check if both are of same type

	left_motor = motor(left_motor_port); 
	right_motor = motor(right_motor_port);
	s = sensor(sensor_port);
    if(s.type()!="ev3-uart-30")  // sensor object s will not be used hereafter
    {
		cerr << "Invalid sensor type. Must be EV3 ultrasonic. Given sensor is of type " << s.type() << endl;
		return 1;
	}    	
//---------------------------------------------------------------------------------------------------------------------------------------------
	// TODO: Check if both were initialised

	left_motor.reset();
	left_motor.set_position(0);
	left_motor.set_run_mode("time");// changed from forever mode to time
	left_motor.set_stop_mode("brake");
	left_motor.set_regulation_mode("on");

	right_motor.reset();
	right_motor.set_position(0);
	right_motor.set_run_mode("time");
	right_motor.set_stop_mode("brake");
	right_motor.set_regulation_mode("on");


nav_msgs::Odometry odom_msg;// msg object for the publisher
ros::Publisher odom_pub("/robot3/odom"/*topic name*/, &odom_msg);
nh.advertise(odom_pub); // advertises that odom_pub is the publisher name
tf::TransformBroadcaster odom_broadcaster; //broadcaster msg for the odometry
tf::TransformBroadcaster scan_broadcaster; // broadcaster for the ultrasonic sensor
nh.subscribe(pose_sub); 

ros::Time current_time, last_time;
	current_time = nh.now();
	last_time = nh.now();


nh.initNode(argv[1]);
odom_broadcaster.init(nh);
//nh.advertiseService(server1);
nh.advertiseService(server2);
while(!nh.connected()) {nh.spinOnce();}

// JUST TO KNOW IF THE NODE IS ALIVE
cout<<"1. Gostraight service\n2.Trace an arc service"<<endl;


        while(1) {
		             // check for incoming messages
		current_time = nh.now();
		geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(t); // odom_quat stores Quaternion cretaed from yaw

		// cout<<" Calculated quat: "<<endl;

		//first, we'll publish the transform over tf
		
		geometry_msgs::TransformStamped odom_trans;//message object
		odom_trans.header.stamp = current_time;
		odom_trans.header.frame_id = map_name;//map_name is just given as map
		const char base_link_name[18] = "/robot3/base_link";
		odom_trans.child_frame_id = base_link_name;

		// cout<<" constructed header"<<endl;
		
		// loading the message object with data
		odom_trans.transform.translation.x = x;
		odom_trans.transform.translation.y = y;
		odom_trans.transform.translation.z = 0.0;// as we are dealing with xy plane
		odom_trans.transform.rotation = odom_quat;// this is quaternion created from yaw angle t

		// cout<<" Constructed full message"<<endl;

		//send the transform
		odom_broadcaster.sendTransform(odom_trans);
		
//.........这里部分代码省略.........

void darthVader(){
    digitalWrite(EnA, HIGH);
    digitalWrite(EnB, HIGH);

    StepMotor.setSpeed(150);
    StepMotor.step(-stepsPerRevolution);
    delay(500);

    //Empirial March
    StepMotor.setSpeed(20);
    StepMotor.step(35);
    delay(60);
    StepMotor.step(-35);
    delay(60);
    StepMotor.step(35);
    delay(60);
    StepMotor.setSpeed(15);
    StepMotor.step(-20);
    delay(50);
    StepMotor.setSpeed(24);
    StepMotor.step(14);
    StepMotor.setSpeed(20);
    StepMotor.step(-35);
    delay(30);
    StepMotor.setSpeed(15);
    StepMotor.step(20);
    delay(50);
    StepMotor.setSpeed(24);
    StepMotor.step(-14);
    StepMotor.setSpeed(20);
    StepMotor.step(65);
    delay(100);
    StepMotor.setSpeed(30);
    StepMotor.step(-50);
    delay(60);
    StepMotor.step(50);
    delay(60);
    StepMotor.step(-50);
    delay(60);
    StepMotor.setSpeed(32);
    StepMotor.step(40);
    delay(50);
    StepMotor.setSpeed(24);
    StepMotor.step(14);
    StepMotor.setSpeed(19);
    StepMotor.step(-30);
    delay(60);
    StepMotor.setSpeed(15);
    StepMotor.step(20);
    delay(50);
    StepMotor.setSpeed(24);
    StepMotor.step(-14);
    StepMotor.setSpeed(20);
    StepMotor.step(70);

    digitalWrite(EnA, LOW);
    digitalWrite(EnB, LOW);

    state_ = 1;
    answer_.data = state_;
    answer_.header.stamp = nh.now();
    answer_pub.publish(&answer_);
}