Пример #1
0
//---------------------------------------------init
void ModuleWidget::init()
{
  m_pUi = new Ui::ModuleWidgetUI();
  m_pUi->setupUi(this);
  m_pUi->m_pModuleName->setText(QString(m_pModule->getRuntimeModuleID().c_str()));

  setAcceptDrops(true);

  m_aTimer.start(TIMER_UNIT); //interval the timer is invoked

  connectModuleSignals(); //call virtual method

  //QObject::connect(m_pUi->m_pRunButton, SIGNAL(pressed()), this, SLOT(onRunButton()));
  //QObject::connect(m_pUi->m_pResetButton, SIGNAL(pressed()), this, SLOT(onResetButton()));
  QObject::connect(&m_aTimer, SIGNAL(timeout()), this, SLOT(onTimer()));

  initInputs();
  initOutputs();

  setContextMenuPolicy(Qt::CustomContextMenu);

  static QBitmap aMask;
  if(aMask.width() == 0) //create the mask only once
  {
    aMask = QBitmap(size());
    aMask.clear();
    QPainter aPainter(&aMask);
    aPainter.setBrush(QBrush(Qt::SolidPattern));
    aPainter.drawEllipse(QPoint(width()/2, height()/2), width()/2, width()/2);
  }
  setMask(aMask);
}
Пример #2
0
void ExactCollisionFeature::computeValuesAndGradients(const boost::shared_ptr<StompTrajectory const>& trajectory,
                                       Eigen::MatrixXd& feature_values,         // num_time_steps x num_features
                                       bool compute_gradients,
                                       std::vector<Eigen::MatrixXd>& gradients, // [num_features] num_joints x num_time_steps
                                       std::vector<int>& validities,             // [num_time_steps] each state valid or not
                                       int thread_id,
                                       int start_timestep,                      // start timestep
                                       int num_time_steps) const
{
  initOutputs(trajectory, feature_values, compute_gradients, gradients, validities);

  collision_detection::CollisionResult result;

  for (int t=start_timestep; t<start_timestep + num_time_steps; ++t)
  {
    collision_world_->checkCollision(collision_request_, result, *collision_robot_,
                                         trajectory->kinematic_states_[t], planning_scene_->getAllowedCollisionMatrix());
    if (result.collision)
    {
      validities[t] = 0;
      feature_values(t, 0) = 1.0;
      if (debug_collisions_)
      {
        collision_detection::CollisionResult::ContactMap::iterator it;
        for (it = result.contacts.begin(); it!= result.contacts.end(); ++it)
        {
          ROS_ERROR("Collision between %s and %s", it->first.first.c_str(), it->first.second.c_str());
        }
      }
    }
  }

//  if (input->per_rollout_data_->collision_models_->isKinematicStateInCollision(*input->per_rollout_data_->kinematic_state_))
//  {
//    state_validity = false;
//    feature_values[0] = 1.0;
//    if (debug_collisions_)
//    {
//      visualization_msgs::MarkerArray arr;
//      std::vector<arm_navigation_msgs::ContactInformation> contact_info;
//      input->per_rollout_data_->collision_models_->getAllCollisionPointMarkers(*input->per_rollout_data_->kinematic_state_,
//                                                                              arr, collision_color, ros::Duration(1.0));
//      input->per_rollout_data_->collision_models_->getAllCollisionsForState(*input->per_rollout_data_->kinematic_state_,
//                                                                           contact_info, 1);
//      for (unsigned int i=0; i<contact_info.size(); ++i)
//      {
//        ROS_INFO("t %02d, Collision between %s and %s",
//                 input->time_index_,
//                 contact_info[i].contact_body_1.c_str(),
//                 contact_info[i].contact_body_2.c_str());
//      }
//      collision_array_viz_pub_.publish(arr);
//    }
//  }
//  else
//  {
//    state_validity = true;
//  }

}
Пример #3
0
// ***************************************************************************
// Initialize internal data structures.
// ***************************************************************************
void MVDetails::init(CollHeap* heap)
{
  // Init the superclass
  DescriptorDetails::init(heap);

  const NAPtrList<QRJBBPtr>& jbbs = descriptor_->getJbbList();
  assertLogAndThrow(CAT_MATCHTST_MVDETAILS, LL_MVQR_FAIL,
                    jbbs.entries() == 1, QRLogicException, 
		    "Support single JBB MVs only for now.");
  // Init the shortcut to the first (and only) JBB in MV descriptors.
  jbbDetails_ = jbbDetailsList_[0];
  // TBD - When we support multi-JBB MVs, we need to loop over the jbb list.
  const QRJBBPtr jbb = jbbs[0];

  // Initialize the several hash tables that map output list elements.
  initOutputs(jbb, heap);

  // Initialize the hash of tables in the descriptor.
  initTables(jbb, heap);
}
Пример #4
0
/****************************************************************************
 *
 * NAME: AppColdStart
 *
 * DESCRIPTION:
 * Entry point for application from boot loader. Initialises system and runs
 * main loop.
 *
 * RETURNS:
 * Never returns.
 *
 ****************************************************************************/
PUBLIC void AppColdStart(void)
{
#if (defined JN5148 || defined JN5168 )
	// TODO - use watch dog and probably disable brownout reset
	vAHI_WatchdogStop();
#endif
#ifdef JN5139
	vAppApiSetBoostMode(TRUE);
#endif

	// Initialise the hopping mode status
	// TODO - move to settings?
	setHopMode(hoppingRxStartup);

	connectObjects();
	RX.ro.version=2;

	// Initialise the system
	vInitSystem();

	// set up the exception handlers
	setExceptionHandlers();


	if(!radioDebug)debugRoute=&pcViaComPort;
	else debugRoute=&pcViaTx;

	if (debugRoute->routeNodes[0] == CONPC)
	{
		// Don't use uart pins for servo op
		initPcComs(&pccoms, CONPC, 0, rxHandleRoutedMessage);
		rxHardware.uart0InUse=TRUE;
	}


	// Initialise the clock
	// TODO - fix this
	/* the jn5148 defaults to 16MHz for the processor,
	   it can be switched to 32Mhz however the servo driving
	   code would need tweaking
	   */
	//bAHI_SetClockRate(3);


	resetType rt=getResetReason();
	if(rt!=NOEXCEPTION)
	{
		dbgPrintf("EXCEPTION %d \r\n",rt);
	}


	// Set handler for incoming data
	setRadioDataCallback(rxHandleRoutedMessage, CONTX);

	// Retrieve the MAC address and log it to the PC
	module_MAC_ExtAddr_s* macptr = (module_MAC_ExtAddr_s*)pvAppApiGetMacAddrLocation();


	// Send init string to PC log rx mac and bound tx mac to pc
	dbgPrintf("rx24 2.10 rx %x %x tx %x %x",macptr->u32H, macptr->u32L,txMACh ,txMACl );


	// Use dio 16 for test sync pulse
	vAHI_DioSetDirection(0, 1 << 16);


	// Set demands to impossible values
	// TODO - fix magic numbers grrr
	int i;
	for (i = 0; i < 20; i++)
	{
		RX.rxDemands[i] = 4096;
		RX.rxMixedDemands[i] = 4096;
	}
	rxHardware.i2cInUse=imu.enabled;
	startIMU(&imu);


	// Set up digital inputs and outputs
	initInputs(&rxHardware);
	initOutputs(&rxHardware);



	if(rxHardware.oneWireEnabled==TRUE)
	{
		// enable onewire sensor bus
		initOneWireBus(&sensorBus1,CONONEWIRE,rxHardware.oneWirePort,rxHandleRoutedMessage);

	}
	if(rxHardware.gpsEnabled==TRUE)
	{
		initNmeaGps(rxHardware.gpsPort, E_AHI_UART_RATE_38400);
	}

	// Setup DIO  for the LED
	vAHI_DioSetDirection(0, rxHardware.ledBit);

	// Initialise Analogue peripherals
	vAHI_ApConfigure(E_AHI_AP_REGULATOR_ENABLE, E_AHI_AP_INT_DISABLE,
			E_AHI_AP_SAMPLE_8, E_AHI_AP_CLOCKDIV_500KHZ, E_AHI_AP_INTREF);

	while (bAHI_APRegulatorEnabled() == 0)
		;


	// Start the servo pwm generator
	setFrameCallback(frameStartEvent);
	setMixCallback(mixEvent);



	startServoPwm(RX.servoUpdateRate);


	// Enter the never ending main handler
	while (1)
	{
		// Process any events
		vProcessEventQueues();
	}
}
Пример #5
0
//---------------------------------------------onModuleOutputUnregistered
void ModuleWidget::onModuleOutputUnregistered(ModuleOutputBase* pOutput)
{
  deInitOutputs();
  initOutputs();
}