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

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

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);
		
//.........这里部分代码省略.........