//---------------------------------------------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);
}
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;
// }
}
// ***************************************************************************
// 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);
}
/****************************************************************************
*
* 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();
}
}
//---------------------------------------------onModuleOutputUnregistered
void ModuleWidget::onModuleOutputUnregistered(ModuleOutputBase* pOutput)
{
deInitOutputs();
initOutputs();
}
