C++ ROS_BREAK函数代码示例

  void ARBundlePublisher::arInit ()
  {



    ROS_INFO("Starting arInit");
    arInitCparam (&cam_param_);
    ROS_INFO ("*** Camera Parameter ***");
    arParamDisp (&cam_param_);

    // load in the object data - trained markers and associated bitmap files
    if ((object = ar_object::read_ObjData (pattern_filename_, &objectnum)) == NULL)
      ROS_BREAK ();
    ROS_INFO("Objectfile num = %d", objectnum);

	// load in the transform data - transform of marker frame wrt center frame
    if ((tfs= ar_transforms::read_Transforms (transforms_filename_, objectnum)) == NULL)
      ROS_BREAK ();
    ROS_INFO("Read in transforms successfully");
    
	

    sz_ = cvSize (cam_param_.xsize, cam_param_.ysize);
#if ROS_VERSION_MINIMUM(1, 9, 0)
// FIXME: Why is this not in the object
    cv_bridge::CvImagePtr capture_; 
#else
// DEPRECATED: Fuerte support ends when Hydro is released
    capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
#endif

	
  }

StageNode::StageNode(int argc, char** argv, bool gui, const char* fname)
{
  this->sim_time.fromSec(0.0);
  this->base_last_cmd.fromSec(0.0);
  double t;
  ros::NodeHandle localn("~");
  if(!localn.getParam("base_watchdog_timeout", t))
    t = 0.2;
  this->base_watchdog_timeout.fromSec(t);

  // We'll check the existence of the world file, because libstage doesn't
  // expose its failure to open it.  Could go further with checks (e.g., is
  // it readable by this user).
  struct stat s;
  if(stat(fname, &s) != 0)
  {
    ROS_FATAL("The world file %s does not exist.", fname);
    ROS_BREAK();
  }

  // initialize libstage
  Stg::Init( &argc, &argv );

  if(gui)
    this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
  else
    this->world = new Stg::World();

  // Apparently an Update is needed before the Load to avoid crashes on
  // startup on some systems.
  // As of Stage 4.1.1, this update call causes a hang on start.
  //this->UpdateWorld();
  this->world->Load(fname);

  // We add our callback here, after the Update, so avoid our callback
  // being invoked before we're ready.
  this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);

  this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
  if (lasermodels.size() != positionmodels.size())
  {
    ROS_FATAL("number of position models and laser models must be equal in "
              "the world file.");
    ROS_BREAK();
  }
  size_t numRobots = positionmodels.size();
  ROS_INFO("found %u position/laser pair%s in the file", 
           (unsigned int)numRobots, (numRobots==1) ? "" : "s");

  this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
  this->odomMsgs = new nav_msgs::Odometry[numRobots];
  this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
  this->colorMsgs = new my_stage::Color[numRobots];
}

int main(int argc, char ** argv)
{
    ros::init(argc, argv, "save_utm_file");

	ros::NodeHandle n;

    std::string file_path = "utm_base_station.yaml";

    ROS_INFO("Opening %s...", file_path.c_str());
    FILE * yaml_file;
    yaml_file = fopen(file_path.c_str() , "w");
    if(yaml_file == NULL)
    {
        ROS_FATAL("Error opnening %s!", file_path.c_str());
        ROS_BREAK();
    }
    ROS_INFO("Done.");

    ROS_INFO("Subscribing to UTM topic...");
    ros::Subscriber sub = n.subscribe("gps/utm", 10, callback);
    ROS_INFO("Done.");

    ros::Time start_time = ros::Time::now();
    bool waiting_for_message = true;

    ROS_INFO("Waiting for UTM message...");
    ros::Rate r(5.0);
    while(ros::ok() && waiting_for_message)
	{
        if(ros::Time::now() - utm_msg.header.stamp < ros::Duration(TIMEOUT))
        {
            fprintf(yaml_file, "base_position:\n  x: %lf\n  y: %lf\n  z: %lf\n\n  covariance: [", utm_msg.position.x, utm_msg.position.y, utm_msg.position.z);
            for(int i=0 ; i<utm_msg.position_covariance.size() ; i++)
            {
                fprintf(yaml_file, "%lf%c", utm_msg.position_covariance[i], (i==utm_msg.position_covariance.size()-1 ? ']' : ','));
            }
            waiting_for_message = false;
        }

        if(ros::Time::now() - start_time > ros::Duration(TIMEOUT))
        {
            ROS_FATAL("Timeout waiting for UTM message. Are you sure the base station node is online?");
            ROS_BREAK();
        }

        ros::spinOnce();
        r.sleep();
	}				

    fclose(yaml_file);
    ROS_INFO("File saved. Goodbye!");

	return 0;
}

// Create a ROS publisher and suscriber for the tacta_belt
int main(int argc, char** argv)
{

  ros::init(argc, argv, "tacta_wrist");
  ros::NodeHandle n;
	ros::Publisher state;
  ros::Rate r(TACTA_BELT_STATE_UPDATE);

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_wrist/input", 1, wrist_cb);
  
  // Change the next line according to your port name and baud rate
  try{ device.open("/dev/ttyUSB0", 19200); }
  catch(cereal::Exception& e)
  {
      ROS_FATAL("Failed to open the serial port!!!");
      ROS_BREAK();
  }
  ROS_INFO("The serial port is opened.");
  
  //On Init Enable B-Cast on All Channels
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_ENB_OUTPUT;
  data[2] = TACTA_BELT_BCAST;
  data[3] = TACTA_BELT_BCAST;
  data[4] = data[0] ^ data[1] ^ data[2] ^ data[3];
  device.write(data, 5);

  ros::spin();

}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "USB2_F_7001_node");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	
	rx_pub = n.advertise<can_msgs::CANFrame>("/can_bus_rx", 10);
    ros::Subscriber tx_sub = n.subscribe<can_msgs::CANFrame>("/can_bus_tx", 10, CANFrameToSend);
	
	std::string port;
	//pn.param<std::string>("port", port, "/dev/ttyUSB0");
	pn.param<std::string>("port", port, "/dev/serial/by-id/usb-LAWICEL_CANUSB_LWVPMVC4-if00-port0"); 
    int bit_rate;
	pn.param("bit_rate", bit_rate, 5);
    
    can_usb_adapter = new USB2_F_7001(&port, &CANFrameReceived);
    
    ROS_INFO("USB2_F_7001 -- Opening CAN bus...");
    if( !can_usb_adapter->openCANBus((USB2_F_7001_BitRate)bit_rate) )
    {
        ROS_FATAL("USB2_F_7001 -- Failed to open the CAN bus!");
		ROS_BREAK();
    }
    ROS_INFO("USB2_F_7001 -- The CAN bus is now open!");
    
	ros::spin();
    
    delete can_usb_adapter;
    
  	return(0);
}

void NavViewPanel::onToolClicked( wxCommandEvent& event )
{
  switch( event.GetId() )
  {
  case ID_MOVE_TOOL:
    {
      delete current_tool_;
      current_tool_ = new MoveTool( this );
    }
    break;

  case ID_GOAL_TOOL:
    {
      delete current_tool_;
      current_tool_ = new PoseTool( this, true );
    }
    break;

  case ID_POSE_TOOL:
    {
      delete current_tool_;
      current_tool_ = new PoseTool( this, false );
    }
    break;

  default:
    ROS_BREAK();
  }

  ROS_ASSERT( current_tool_ );
}

void SerialServer::readParameters()
{
  std::string port_name, port_type;
  LxSerial::PortType lx_port_type;
  int baud_rate;
  double watchdog_interval;
  
  ROS_INFO("Reading parameters");
  
  ROS_ASSERT(nh_.getParam("port_name", port_name));
  ROS_ASSERT(nh_.getParam("port_type", port_type));
  ROS_ASSERT(nh_.getParam("baud_rate", baud_rate));
  nh_.param<double>("watchdog_interval", watchdog_interval, 10);

  if (port_type == "RS232")
    lx_port_type = LxSerial::RS232;
  else if (port_type == "RS485_FTDI")
    lx_port_type = LxSerial::RS485_FTDI;
  else if (port_type == "RS485_EXAR")
    lx_port_type = LxSerial::RS485_EXAR;
  else if (port_type == "RS485_SMSC")
    lx_port_type = LxSerial::RS485_SMSC;
  else if (port_type == "TCP")
    lx_port_type = LxSerial::TCP;
  else
  {
    ROS_FATAL_STREAM("Unknown port type " << port_type);
    ROS_BREAK();
    return;
  }

  ROS_ASSERT(serial_port_.port_open(port_name, lx_port_type));
  ROS_ASSERT(serial_port_.set_speed_int(baud_rate));
  ROS_ASSERT(watchdog_.set(watchdog_interval));
}

const Path&
OccupancyMap::prepareShortestPaths(double x, double y,
                                   double min_distance, double max_distance,
                                   double max_occ_dist,
                                   bool allow_unknown) {
  endpoints_.clear();

  if (map_ == NULL) {
    ROS_WARN("OccupancyMap::prepareShortestPaths() Map not set");
    return endpoints_;
  }

  if (map_->max_occ_dist < max_occ_dist) {
    ROS_ERROR("OccupancyMap::prepareShortestPaths() CSpace has been calculated "
              "up to %f, but max_occ_dist=%.2f",
              map_->max_occ_dist, max_occ_dist);
    ROS_BREAK();
  }

  initializeSearch(x, y);

  Node curr_node;
  while (nextNode(max_occ_dist, &curr_node, allow_unknown)) {
    double node_dist = curr_node.true_cost * map_->scale;
    if (min_distance <= node_dist && node_dist < max_distance) {
      float x = MAP_WXGX(map_, curr_node.coord.first);
      float y = MAP_WYGY(map_, curr_node.coord.second);
      endpoints_.push_back(Eigen::Vector2f(x, y));
    } else if (node_dist > max_distance) {
      break;
    }
  }
  return endpoints_;
}

int
main (int argc, char **argv)
{
  ros::init (argc, argv, "pgr_camera");

  typedef dynamic_reconfigure::Server < pgr_camera_driver::PGRCameraConfig > Server;
  Server server;

  try
  {
    boost::shared_ptr < PGRCameraNode > pn (new PGRCameraNode (ros::NodeHandle(),ros::NodeHandle("~")));

    Server::CallbackType f = boost::bind (&PGRCameraNode::configure, pn, _1, _2);
    server.setCallback (f);

    ros::spin ();

  } catch (std::runtime_error & e)
  {
    ROS_FATAL ("Uncaught exception: '%s', aborting.", e.what ());
    ROS_BREAK ();
  }

  return 0;

}

    aiReturn Seek( size_t offset, aiOrigin origin)
    {
      uint8_t* new_pos = 0;
      switch (origin)
      {
      case aiOrigin_SET:
        new_pos = res_.data.get() + offset;
        break;
      case aiOrigin_CUR:
        new_pos = pos_ + offset; // TODO is this right?  can offset really not be negative
        break;
      case aiOrigin_END:
        new_pos = res_.data.get() + res_.size - offset; // TODO is this right?
        break;
      default:
        ROS_BREAK();
      }

      if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
      {
        return aiReturn_FAILURE;
      }

      pos_ = new_pos;
      return aiReturn_SUCCESS;
    }

void AudioProcessor::checkForErrors(FMOD_RESULT result)
{
  if (result != FMOD_OK)
  {
    ROS_ERROR("FMOD error! (%d) %s", result, FMOD_ErrorString(result));
    ROS_BREAK();
  }
}

  void spin() 
  {
    initRCservo();
    rcservo_EnterPlayMode(); 
      
    com4_ics_init();
    ServoLibrary::iterator i;
    for(i = servos_.begin(); i != servos_.end(); ++i)
    {
      //std::string& joint_name = i->first;
      Servo& servo = i->second;
      if (servo.bus_ == Servo::COM4)
      {
        usleep(1);
        //com4_ics_pos(servo.channel_, 8193);
				// TODO: there is a command to tell the servo to lock into its current position, but
				//       doesnt seem to work.
      }
    }
    
    running_ = true;
            
    static pthread_t controlThread_;
    static pthread_attr_t controlThreadAttr_;

    int rv;
    if (playaction_thread_)
    {
    	if ((rv = pthread_create(&controlThread_, &controlThreadAttr_, playActionThread, this)) != 0)
      {
        ROS_FATAL("Unable to create control thread: rv = %d", rv);
        ROS_BREAK();
      }
    }
    
    ros::spin();

    running_ = false;
    if (playaction_thread_)
    	pthread_join(controlThread_, NULL);//(void **)&rv);
    rcservo_Close();
    
    for(i = servos_.begin(); i != servos_.end(); ++i)
    {
      //std::string& joint_name = i->first;
      Servo& servo = i->second;
      if (servo.bus_ == Servo::COM4)
      {
        com4_ics_set_stretch(servo.channel_, 4);
        usleep(1);
        com4_ics_set_speed(servo.channel_, 64);
        usleep(1);
        com4_ics_pos(servo.channel_, 0);
        usleep(1);
      }
    } 
    com4_ics_close();
  }

int main(int argc, char ** argv)
{
	ros::init(argc, argv, "rtk_base_station");

	ROS_INFO("RTKlib for ROS Base Station Edition");

	ros::NodeHandle n;
	ros::NodeHandle pn("~");

    	std::string port;
    	pn.param<std::string>("port", port, "/dev/ttyACM1");
	int baudrate;
	pn.param("baudrate", baudrate, 115200);

	cereal::CerealPort serial_gps;
		
    	try{ serial_gps.open(port.c_str(), baudrate); }
	catch(cereal::Exception& e)
    	{
        	ROS_FATAL("RTK -- Failed to open the serial port!!!");
        	ROS_BREAK();
    	}

	char buffer[350];
	int count;
	
	buffer[108]=0;
	buffer[0]='l';
	buffer[1]='s';
	buffer[2]='e';
	buffer[3]=1;

    	ros::Publisher pub = n.advertise<std_msgs::ByteMultiArray>("base_station/gps/raw_data", 100);

	ROS_INFO("RTK -- Streaming data...");

	while(ros::ok())
	{
        	try{ count = serial_gps.read(buffer, 300, 1000); }
		catch(cereal::TimeoutException& e)
		{
		    ROS_WARN("RTK -- Failed to get data from GPS!");
		}

		std_msgs::ByteMultiArray raw_msg;
		
        	for(int i=0 ; i<count ; i++)
		{
			raw_msg.data.push_back(buffer[i]);
		}
		
		pub.publish(raw_msg);
		ros::Duration(0.1).sleep();	
	}				
	
	return 0;
}

void setRetryTimeout(ros::WallDuration timeout)
{
  if (timeout < ros::WallDuration(0))
  {
    ROS_FATAL("retry timeout must not be negative.");
    ROS_BREAK();
  }
  g_retry_timeout = timeout;
}

PollSet::PollSet()
: sockets_changed_(false)
{
	if ( create_signal_pair(signal_pipe_) != 0 ) {
        ROS_FATAL("create_signal_pair() failed");
        ROS_BREAK();
	}
  addSocket(signal_pipe_[0], boost::bind(&PollSet::onLocalPipeEvents, this, _1));
  addEvents(signal_pipe_[0], POLLIN);
}