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

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

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

}

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

void init(const M_string& remappings)
{
  M_string::const_iterator it = remappings.find("__master");
  if (it != remappings.end())
  {
    g_uri = it->second;
  }

  if (g_uri.empty())
  {
    char *master_uri_env = NULL;
    #ifdef _MSC_VER
      _dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
    #else
      master_uri_env = getenv("ROS_MASTER_URI");
    #endif
    if (!master_uri_env)
    {
      ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
                 "type the following or (preferrably) add this to your " \
                 "~/.bashrc file in order set up your " \
                 "local machine as a ROS master:\n\n" \
                 "export ROS_MASTER_URI=http://localhost:11311\n\n" \
                 "then, type 'roscore' in another shell to actually launch " \
                 "the master program.");
      ROS_BREAK();
    }

    g_uri = master_uri_env;

#ifdef _MSC_VER
    // http://msdn.microsoft.com/en-us/library/ms175774(v=vs.80).aspx
    free(master_uri_env);
#endif
  }

  // Split URI into
  if (!network::splitURI(g_uri, g_host, g_port))
  {
    ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
    ROS_BREAK();
  }
}

  /** Get video mode.
   *
   *  @param camera points to DC1394 camera struct
   *  @param[in,out] video_mode Config parameter for this option,
   *                      updated if the camera does not support the
   *                      requested value
   *  @return corresponding dc1394video_mode_t enum value selected
   */
  dc1394video_mode_t getVideoMode(dc1394camera_t *camera,
                                  std::string &video_mode)
  {
    for (int vm = DC1394_VIDEO_MODE_MIN;
         vm <= DC1394_VIDEO_MODE_MAX;
         ++vm)
      {
        if (video_mode_names_[vm-DC1394_VIDEO_MODE_MIN] == video_mode)
          {
            // found the requested mode
            dc1394video_modes_t vmodes;
            dc1394error_t err =
              dc1394_video_get_supported_modes(camera, &vmodes);
            if (err != DC1394_SUCCESS)
              {
                ROS_FATAL("unable to get supported video modes");
                // TODO raise exception
                return (dc1394video_mode_t) 0;
              }

            // see if requested mode is available
            for (uint32_t i = 0; i < vmodes.num; ++i)
              {
                if (vmodes.modes[i] == vm)
                  return (dc1394video_mode_t) vm; // yes: success
              }

            // requested mode not available, revert to current mode of camera
            ROS_ERROR_STREAM("Video mode " << video_mode
                             << " not supported by this camera");
            dc1394video_mode_t current_mode;
            err = dc1394_video_get_mode(camera, &current_mode);
            if (err != DC1394_SUCCESS)
              {
                ROS_FATAL("unable to get current video mode");
                // TODO raise exception
                return (dc1394video_mode_t) 0;
              }

            // TODO list available modes

            // change video_mode parameter to show current mode of camera
            video_mode = videoModeName(current_mode);
            return current_mode;
          }
      }

    // request video mode does not match any known string
    ROS_FATAL_STREAM("Unknown video_mode:" << video_mode);
    ROS_BREAK();
    // TODO raise exception
    //CAM_EXCEPT(camera1394::Exception, "Unsupported video_mode");
    return (dc1394video_mode_t) 0;
  }

CANUSB::CANUSB(std::string * serial_port_name, boost::function<void(std::string*)> f) : serial_port_()
{
    open_ = false;
    
    try{ serial_port_.open((char*)serial_port_name->c_str(), 115200); }
	catch(cereal::Exception& e)
	{
		ROS_FATAL("CANUSB - %s - Failed to open the serial port!!!", __FUNCTION__);
        ROS_BREAK();
	}
    
    receivedFrameCallback = f;
}

void OccupancyMap::initializeSearch(double startx, double starty) {
  starti_ = MAP_GXWX(map_, startx);
  startj_ = MAP_GYWY(map_, starty);

  if (!MAP_VALID(map_, starti_, startj_)) {
    ROS_ERROR("OccupancyMap::initializeSearch() Invalid starting position");
    ROS_BREAK();
  }

  int ncells = map_->size_x * map_->size_y;
  if (ncells_ != ncells) {
    ncells_ = ncells;
    costs_.reset(new float[ncells]);
    prev_i_.reset(new int[ncells]);
    prev_j_.reset(new int[ncells]);
  }

  // TODO: Return to more efficient lazy-initialization
  // // Map is large and initializing costs_ takes a while.  To speedup,
  // // partially initialize costs_ in a rectangle surrounding start and stop
  // // positions + margin.  If you run up against boundary, initialize the rest.
  // int margin = 120;
  // init_ul_ = make_pair(max(0, min(starti_, stopi) - margin),
  //                      max(0, min(startj_, stopj) - margin));
  // init_lr_ = make_pair(min(map_->size_x, max(starti_, stopi) + margin),
  //                      min(map_->size_y, max(startj_, stopj) + margin));
  // for (int j = init_ul.second; j < init_lr.second; ++j) {
  //   for (int i = init_ul.first; i < init_lr.first; ++i) {
  //     int ind = MAP_INDEX(map_, i, j);
  //     costs_[ind] = std::numeric_limits<float>::infinity();
  //   }
  // }
  // full_init_ = false;

  for (int i = 0; i < ncells_; ++i) {
    costs_[i] = std::numeric_limits<float>::infinity();
    prev_i_[i] = -1;
    prev_j_[i] = -1;
  }

  int start_ind = MAP_INDEX(map_, starti_, startj_);
  costs_[start_ind] = 0.0;
  prev_i_[starti_] = starti_;
  prev_j_[startj_] = startj_;

  Q_.reset(new set<Node, NodeCompare>());
  Q_->insert(Node(make_pair(starti_, startj_), 0.0, 0.0));

  stopi_ = -1;
  stopj_ = -1;
}

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

  ros::init(argc, argv, "tacta_fingers");
  ros::NodeHandle n;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_grippers/input", 1, grippers_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 has been opened.");

  //On Init Enable B-Cast on All Channels
  data_amp[0] = TACTA_BELT_ADDRESS;
  data_amp[1] = TACTA_BELT_ENB_OUTPUT;
  data_amp[2] = TACTA_BELT_BCAST;
  data_amp[3] = TACTA_BELT_BCAST;
  data_amp[4] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3];
  device.write(data_amp, 5);

  ros::Rate r(100);

  for (int i = 0; i < 100; i++){
      //Right Hand output struct
      rh_rf_output[i] = 0;
      rh_lf_output[i] = 0;

      //Left Hand output struct
      lh_rf_output[i] = 0;
      lh_lf_output[i] = 0;
  }

  while(ros::ok()){

      tacta_output();

      ros::spinOnce();

      r.sleep();

  }

  ros::spin();

}