Beispiel #1
0
IceApp::sensorinfo_t 
IceApp::connectToSensor(const std::string &sensor_name) const
{
  Ice::PropertiesPtr props = this->communicator()->getProperties();
  Ice::LoggerPtr log = this->communicator()->getLogger();

  try
    {
      log->print(string("Connecting to ") + sensor_name + " sensor");
      const string prop_name = sensor_name + ".proxy";
      string sensor_str_proxy = props->getProperty(prop_name);
      sensors::SensorGroupPrx sensor_prx;
      if(sensor_str_proxy.empty())
	log->warning(prop_name 
		     + string(" property is not set. No ")
		     + sensor_name + ".");
      else
	{
	  Ice::ObjectPrx objprx = 
	    this->communicator()->stringToProxy(sensor_str_proxy);
	  sensor_prx = sensors::SensorGroupPrx::checkedCast(objprx);
	  if(!sensor_prx)
	    log->warning(string("Can not connect to ")
			 + sensor_name 
			 + string(" sensor. No ")
			 + sensor_name + ".");
	  else
	    {
	      string msg = "Connected to sensor group ";
	      log->print(msg + sensor_name);
	      sensors::SensorDescriptionSeq descr = 
		sensor_prx->getSensorDescription();
	      for(sensors::SensorDescriptionSeq::const_iterator d = descr.begin();
		  d != descr.end(); ++d)
		{
		  ostringstream os;
		  os << "  Vendor id: " << d->vendorid << endl;
		  os << "  Description: " << d->description << endl;
		  os << "  Id: " << d->id << endl;
		  os << "  Min value: " << d->minvalue << endl;
		  os << "  Max value: " << d->maxvalue << endl;
		  os << "  Recommended refresh rate, Hz: " << d->refreshrate;
		  log->print(os.str());
		}
	      return make_pair(sensor_prx, descr);
	    }
	}
    }
  catch(const Ice::Exception& ex)
    {
      ostringstream os;
      os << "Can not connect to " << sensor_name << " sensor. No "
	 << sensor_name + ".\n";
      ex.ice_print(os);
      log->warning(os.str());
    }

  return make_pair(sensors::SensorGroupPrx(), 
		   sensors::SensorDescriptionSeq());
}
Beispiel #2
0
IceApp::actuatorinfo_t 
IceApp::connectToActuator(const std::string &actuator_name) const
{
  Ice::PropertiesPtr props = this->communicator()->getProperties();
  Ice::LoggerPtr log = this->communicator()->getLogger();

  try
    {
      log->print(string("Connecting to ") + actuator_name + " actuators");
      const string prop_name = actuator_name + ".proxy";
      string actuator_str_proxy = props->getProperty(prop_name);
      actuators::ActuatorGroupPrx actuator_prx;
      if(actuator_str_proxy.empty())
	log->warning(prop_name 
		     + string(" property is not set. No ")
		     + actuator_name + ".");
      else
	{
	  Ice::ObjectPrx objprx = 
	    this->communicator()->stringToProxy(actuator_str_proxy);
	  actuator_prx = 
	    actuators::ActuatorGroupPrx::checkedCast(objprx);
	  if(!actuator_prx)
	    log->warning(string("Can not connect to ")
			 + actuator_name 
			 + string(" actuator. No ")
			 + actuator_name + ".");
	  else
	    {
	      string msg = "Connected to actuator group ";
	      log->print(msg + actuator_name);
	      actuators::ActuatorDescriptionSeq descr = 
		actuator_prx->getActuatorDescription();
	      for(actuators::ActuatorDescriptionSeq::const_iterator d = descr.begin();
		  d != descr.end(); ++d)
		{
		  ostringstream os;
		  os << "  Vendor id: " << d->vendorid << endl;
		  os << "  Description: " << d->description << endl;
		  os << "  Id: " << d->id << endl;
		  log->print(os.str());
		}
	      return make_pair(actuator_prx, descr);
	    }
	}
    }
  catch(const Ice::Exception &ex)
    {
      std::ostringstream os;
      os << "Can not connect to actuator " + actuator_name + ".\n";
      ex.ice_print(os);
      log->warning(os.str());
    }

  return make_pair(actuators::ActuatorGroupPrx(), 
		   actuators::ActuatorDescriptionSeq());
}
Beispiel #3
0
IceInternal::ACMConfig::ACMConfig(const Ice::PropertiesPtr& p,
                                  const Ice::LoggerPtr& l,
                                  const string& prefix,
                                  const ACMConfig& dflt)
{
    string timeoutProperty;
    if((prefix == "Ice.ACM.Client" || prefix == "Ice.ACM.Server") && p->getProperty(prefix + ".Timeout").empty())
    {
        timeoutProperty = prefix; // Deprecated property.
    }
    else
    {
        timeoutProperty = prefix + ".Timeout";
    };

    this->timeout = IceUtil::Time::seconds(p->getPropertyAsIntWithDefault(timeoutProperty,
                                                                          static_cast<int>(dflt.timeout.toSeconds())));
    int hb = p->getPropertyAsIntWithDefault(prefix + ".Heartbeat", static_cast<int>(dflt.heartbeat));
    if(hb >= static_cast<int>(ICE_ENUM(ACMHeartbeat, HeartbeatOff)) &&
       hb <= static_cast<int>(ICE_ENUM(ACMHeartbeat, HeartbeatAlways)))
    {
        this->heartbeat = static_cast<Ice::ACMHeartbeat>(hb);
    }
    else
    {
        l->warning("invalid value for property `" + prefix + ".Heartbeat" + "', default value will be used instead");
        this->heartbeat = dflt.heartbeat;
    }

    int cl = p->getPropertyAsIntWithDefault(prefix + ".Close", static_cast<int>(dflt.close));
    if(cl >= static_cast<int>(ICE_ENUM(ACMClose, CloseOff)) &&
       cl <= static_cast<int>(ICE_ENUM(ACMClose, CloseOnIdleForceful)))
    {
        this->close = static_cast<Ice::ACMClose>(cl);
    }
    else
    {
        l->warning("invalid value for property `" + prefix + ".Close" + "', default value will be used instead");
        this->close = dflt.close;
    }
}
Beispiel #4
0
int 
IceApp::run(int argc, char *argv[])
{
  RouterHelperPtr rh;
  Glacier2::RouterPrx defaultRouter;
  Ice::PropertiesPtr props = this->communicator()->getProperties();
  Ice::LoggerPtr log = this->communicator()->getLogger();

  // Try to create router with firewall support using Glacier service
  try
    {
      rh = new RouterHelper(this->communicator());
      defaultRouter = rh->getRouter();
    }
  catch(const Ice::Exception& ex)
    {
      ostringstream os;
      ex.ice_print(os);
      log->warning(os.str());
    }

  // Create object adapter for sensor callback and keyboard sensor servants
  Ice::ObjectAdapterPtr adapter;
  if(defaultRouter)
    adapter = communicator()->createObjectAdapterWithRouter("Cockpit", 
							    defaultRouter);
  else
    adapter = communicator()->createObjectAdapter("Cockpit");

  if(this->visuals.init(log, props))
    {
      log->error("Visuals initialization failed");
      return EXIT_FAILURE;
    }

  Ice::ObjectPrx objprx;

  // Initialize keyboard sensor (and admin) servants
  KeyboardAdminIPtr keyboard_admin = new KeyboardAdminI();
  objprx = adapter->add(keyboard_admin, 
			this->communicator()->stringToIdentity("keyboard-sensor-admin"));
  admin::StatePrx keyboard_admin_prx = 
    admin::StatePrx::uncheckedCast(objprx); 

  this->keyboard_sensor = new KeyboardSensorI(keyboard_admin_prx);
  keyboard_admin->setSensorGroup(this->keyboard_sensor);
  objprx = 
    adapter->add(this->keyboard_sensor, 
		 this->communicator()->stringToIdentity("keyboard-sensor"));

  adapter->activate();

  admin::StatePrx state_prx;

  // Initialize actuators list

  // Our chassis has two motors
  actuatorinfo_t cactinfo = this->connectToActuator("Chassis");
  actuators::ActuatorGroupPrx cactuator_prx = cactinfo.first;
  //actuators::ActuatorDescriptionSeq cactdescr = actinfo.second;

  // Servo motor to rotate the head with cameras and front sonar
  actuatorinfo_t hactinfo = this->connectToActuator("Head");
  actuators::ActuatorGroupPrx hactuator_prx = hactinfo.first;
  //actuators::ActuatorDescriptionSeq hactdescr = actinfo.second;

  // Initialize joystick callback servant
  sensorinfo_t info = this->connectToSensor("Joystick");
  sensors::SensorGroupPrx sensor_prx = info.first;
  sensors::SensorDescriptionSeq descr = info.second;
  ActuatorControllerPtr joy_chassis_ctl;
  ActuatorControllerPtr joy_head_ctl;
  if(sensor_prx && !descr.empty())
    {
      actctlkey_t key = std::make_pair(descr[0].id, descr[0].type);
      if(cactuator_prx)
	{
	  joy_chassis_ctl.reset(new JoystickChassisCtl(cactuator_prx));
	  this->actuator_map.insert(std::make_pair(key, 
						   joy_chassis_ctl.get()));
	  this->chassis_state_prx = cactuator_prx->getStateInterface();
	  this->chassis_state_prx->start();
	}
      if(hactuator_prx)
	{
	  joy_head_ctl.reset(new JoystickHeadCtl(hactuator_prx));
	  this->actuator_map.insert(std::make_pair(key, 
						   joy_head_ctl.get()));
	  state_prx = hactuator_prx->getStateInterface();
	  state_prx->start();
	}
      SensorFrameReceiverIPtr joystick_callback = 
	new SensorFrameReceiverI(descr[0].type,
				 descr[0].minvalue,
				 descr[0].maxvalue,
				 &this->buffer_queue, 
				 &this->visuals.tile_manager,
				 &this->visuals.sensor_animations_map,
				 &this->actuator_map);
      objprx = 
	adapter->add(joystick_callback, rh->makeId("joystickcallback"));
      sensors::SensorFrameReceiverPrx joystick_callback_prx = 
	sensors::SensorFrameReceiverPrx::uncheckedCast(objprx); 
      sensor_prx->setSensorReceiver(joystick_callback_prx);
      state_prx = sensor_prx->getStateInterface();
      state_prx->start();
    }

  // Initialize keyboard callback servant
  info = this->connectToSensor("Keyboard");
  sensor_prx = info.first;
  descr = info.second;
  ActuatorControllerPtr kbd_chassis_ctl;
  ActuatorControllerPtr kbd_head_ctl;
  if(sensor_prx && !descr.empty())
    {
      actctlkey_t key = std::make_pair(descr[0].id, descr[0].type);
      if(cactuator_prx)
	{
	  kbd_chassis_ctl.reset(new KeyboardChassisCtl(cactuator_prx));
	  this->actuator_map.insert(std::make_pair(key, 
						   kbd_chassis_ctl.get()));
	  state_prx = cactuator_prx->getStateInterface();
	  state_prx->start();
	}
      if(hactuator_prx)
	{
	  kbd_head_ctl.reset(new KeyboardHeadCtl(hactuator_prx));
	  this->actuator_map.insert(std::make_pair(key, 
						   kbd_head_ctl.get()));
	  state_prx = hactuator_prx->getStateInterface();
	  state_prx->start();
	}

      SensorFrameReceiverIPtr kbd_callback = 
	new SensorFrameReceiverI(descr[0].type,
				 descr[0].minvalue,
				 descr[0].maxvalue,
				 &this->buffer_queue, 
				 &this->visuals.tile_manager,
				 &this->visuals.sensor_animations_map,
				 &this->actuator_map);
      objprx = 
	adapter->add(kbd_callback, 
		     this->communicator()->stringToIdentity("kbdcallback"));
      sensors::SensorFrameReceiverPrx kbd_callback_prx = 
	sensors::SensorFrameReceiverPrx::uncheckedCast(objprx); 
      sensor_prx->setSensorReceiver(kbd_callback_prx);
      state_prx = sensor_prx->getStateInterface();
      state_prx->start();
    }

  // Initialize sonars callback servant
  info = this->connectToSensor("Sonars");
  sensor_prx = info.first;
  descr = info.second;
  if(sensor_prx && !descr.empty())
    {
      SensorFrameReceiverIPtr sonars_callback = 
	new SensorFrameReceiverI(descr[0].type,
				 descr[0].minvalue,
				 descr[0].maxvalue,
				 &this->buffer_queue, 
				 &this->visuals.tile_manager,
				 &this->visuals.sensor_animations_map,
				 NULL);
      objprx = 
	adapter->add(sonars_callback, rh->makeId("sonarscallback"));
      sensors::SensorFrameReceiverPrx sonars_callback_prx = 
	sensors::SensorFrameReceiverPrx::uncheckedCast(objprx); 
      sensor_prx->setSensorReceiver(sonars_callback_prx);
      admin::StatePrx state_prx = sensor_prx->getStateInterface();
      state_prx->start();
    }

  // Initialize compass callback servant
  info = this->connectToSensor("Compass");
  sensor_prx = info.first;
  descr = info.second;
  if(sensor_prx && !descr.empty())
    {
      SensorFrameReceiverIPtr compass_callback = 
	new SensorFrameReceiverI(descr[0].type,
				 descr[0].minvalue,
				 descr[0].maxvalue,
				 &this->buffer_queue, 
				 &this->visuals.tile_manager,
				 &this->visuals.sensor_animations_map,
				 NULL);
      objprx = 
	adapter->add(compass_callback, rh->makeId("compasscallback"));
      sensors::SensorFrameReceiverPrx compass_callback_prx = 
	sensors::SensorFrameReceiverPrx::uncheckedCast(objprx); 
      sensor_prx->setSensorReceiver(compass_callback_prx);
      admin::StatePrx state_prx = sensor_prx->getStateInterface();
      state_prx->start();
    }

  // Initialize camera callback servant
  info = this->connectToSensor("Camera");
  sensor_prx = info.first;
  descr = info.second;
  if(sensor_prx && !descr.empty())
    {
      const std::string prop_name = "Decoding.Pipeline";
      std::string dec_pipeline = props->getProperty(prop_name);
      if(dec_pipeline.empty())
	{
	  log->warning(prop_name + " property not set. No video.");
	}
      else
	{
	  this->video_decoder.initAndStart(argc, argv, 
					   dec_pipeline.c_str(),
					   this->visuals.video_painters,
					   &this->buffer_queue);
				       
	  SensorFrameReceiverIPtr camera_callback = 
	    new SensorFrameReceiverI(descr[0].type,
				     descr[0].minvalue,
				     descr[0].maxvalue,
				     &this->buffer_queue, 
				     &this->visuals.tile_manager,
				     &this->visuals.sensor_animations_map,
				     NULL);
	  objprx = 
	    adapter->add(camera_callback, rh->makeId("cameracallback"));
	  sensors::SensorFrameReceiverPrx camera_callback_prx = 
	    sensors::SensorFrameReceiverPrx::uncheckedCast(objprx); 
	  sensor_prx->setSensorReceiver(camera_callback_prx);
	  state_prx = sensor_prx->getStateInterface();
	  state_prx->start();
	}
    }
  
  this->mainloop();

  // Be nice and stop chassis motors on exit
  if(this->chassis_state_prx)
    {
      this->chassis_state_prx->stop();
      this->chassis_state_prx = 0;
    }

  return EXIT_SUCCESS;
}