void computeLinearLoad(const uint32_t nr_of_items, uint32_t* blocks, uint32_t* threads_per_block)
{
// if (nr_of_items <= cMAX_NR_OF_BLOCKS)
// {
// *blocks = nr_of_items;
// *threads_per_block = 1;
// }
// else
// {
if(nr_of_items == 0)
{
LOGGING_WARNING(
Gpu_voxels_helpers,
"Number of Items is 0. Blocks and Threads per Block is set to 1. Size 0 would lead to a Cuda ERROR" << endl);
*blocks = 1;
*threads_per_block = 1;
return;
}
if (nr_of_items <= cMAX_NR_OF_BLOCKS * cMAX_THREADS_PER_BLOCK)
{
*blocks = (nr_of_items + cMAX_THREADS_PER_BLOCK - 1)
/ cMAX_THREADS_PER_BLOCK; // calculation replaces a ceil() function
*threads_per_block = cMAX_THREADS_PER_BLOCK;
}
else
{
/* In this case the kernel must perform multiple runs because
* nr_of_items is larger than the gpu can handle at once.
* To overcome this limit, use standard parallelism offsets
* as when programming host code (increment by the number of all threads
* running). Use something like
*
* uint32_t i = blockIdx.x * blockDim.x + threadIdx.x;
*
* while (i < nr_of_items)
* {
* // perform some kernel operations here
*
* // increment by number of all threads that are running
* i += blockDim.x * gridDim.x;
* }
*
* CAUTION: currently cMAX_NR_OF_BLOCKS is 64K, although
* GPUs with SM >= 3.0 support up to 2^31 -1 blocks in a grid!
*/
LOGGING_ERROR(
Gpu_voxels_helpers,
"computeLinearLoad: Number of Items " << nr_of_items << " exceeds the limit cMAX_NR_OF_BLOCKS * cMAX_THREADS_PER_BLOCK = " << (cMAX_NR_OF_BLOCKS*cMAX_THREADS_PER_BLOCK) << "! This number of items cannot be processed in a single invocation." << endl);
*blocks = cMAX_NR_OF_BLOCKS;
*threads_per_block = cMAX_THREADS_PER_BLOCK;
}
}
void HardwareCanSinkPeak::set(const CanMessageStamped::Ptr& msg)
{
if (!m_can_device)
{
LOGGING_ERROR(CAN, "CAN device is not available, ignoring received message." << endl);
return;
}
if (!msg || ((*msg)->id == 0 && (*msg)->dlc == 0 && (*msg)->rtr == 0))
{
LOGGING_WARNING(CAN, "No regular message received." << endl);
return;
}
m_can_device->Send(**msg);
}
tHardwareCanSink * tHardwareCanSinkFactory::Construct(std::string specifier)
{
icl_core::SchemeParser parser;
if (specifier == "")
{
LOGGING_DEBUG(icl_hardware::can::CAN, "No specifier was set, read settings from configfile" << "\n");
specifier = interpretConfigFile();
}
if (parser.parseScheme(specifier))
{
// convert to lowercase
std::string lspecifier(parser.getSchemeResult().specifier);
for (unsigned int i = 0; i < lspecifier.size(); ++i)
{
lspecifier[i] = tolower(lspecifier[i]);
}
LOGGING_DEBUG(icl_hardware::can::CAN, "tHardwareCanSinkFactory parser result: \n");
LOGGING_DEBUG(icl_hardware::can::CAN, " scheme_name: "<< parser.getSchemeResult().scheme_name << "\n");
LOGGING_DEBUG(icl_hardware::can::CAN, " specifier: "<< parser.getSchemeResult().specifier << "\n");
LOGGING_DEBUG(icl_hardware::can::CAN, " anchor: "<< parser.getSchemeResult().anchor << "\n");
LOGGING_DEBUG(icl_hardware::can::CAN, " #queries: "<< parser.getSchemeResult().queries.size() << icl_core::logging::endl);
if (parser.getSchemeResult().scheme_type == icl_core::FileScheme)
{
tHardwareCanSinkCanfile * hardware_data_sink = new tHardwareCanSinkCanfile();
icl_core::QueryList query_list = parser.getSchemeResult().queries;
unsigned int baud_rate = 0;
for (unsigned int i=0; i<query_list.size(); ++i)
{
if (query_list[i].name == "baud")
{
baud_rate = atoi(query_list[i].value.c_str());
}
}
if (baud_rate != 0)
{
hardware_data_sink->Open(parser.getSchemeResult().specifier, baud_rate);
}
else
{
hardware_data_sink->Open(parser.getSchemeResult().specifier);
}
return hardware_data_sink;
}
else if (parser.getSchemeResult().scheme_type == icl_core::OtherScheme)
{
if (parser.getSchemeResult().scheme_name == "console://")
{
if (icl_core::config::getDefault<bool>("/icl_hardware_can/use_interpret", false))
{
icl_core::String can_matrix = icl_core::config::getDefault<icl_core::String>("/icl_hardware_can/can_mask", "");
if (can_matrix != "")
{
tHardwareCanSinkInterpretMessage * hardware_data_sink = new tHardwareCanSinkInterpretMessage();
hardware_data_sink->Open(parser.getSchemeResult().specifier, can_matrix);
return hardware_data_sink;
}
else
{
LOGGING_ERROR(icl_hardware::can::CAN, "Interpretation sink must be used together with CAN mask!" << icl_core::logging::endl);
}
}
else
{
tHardwareCanSinkConsole * hardware_data_sink = new tHardwareCanSinkConsole();
hardware_data_sink->Open(parser.getSchemeResult().specifier);
return hardware_data_sink;
}
}
if (parser.getSchemeResult().scheme_name == "can://")
{
#ifdef _SYSTEM_POSIX_
tHardwareCanSinkPeak * hardware_data_sink = new tHardwareCanSinkPeak();
icl_core::QueryList query_list = parser.getSchemeResult().queries;
unsigned int baud_rate = 0;
for (unsigned int i=0; i<query_list.size(); ++i)
{
if (query_list[i].name == "baud")
{
baud_rate = atoi(query_list[i].value.c_str());
}
}
if (baud_rate != 0)
{
hardware_data_sink->Open(parser.getSchemeResult().specifier, baud_rate);
}
else
{
hardware_data_sink->Open(parser.getSchemeResult().specifier);
}
return hardware_data_sink;
#else
LOGGING_WARNING(icl_hardware::can::CAN, "can:// devices only available on posix platforms.");
return NULL;
#endif
}
}
LOGGING_ERROR(icl_hardware::can::CAN, "Could not distinguish HardwareSink from scheme \"" << specifier << "\"!" << icl_core::logging::endl);
}
else
{
LOGGING_ERROR(icl_hardware::can::CAN, "Failed to parse scheme \"" << specifier << "\"!" << icl_core::logging::endl);
}
return NULL;
}
void MissionControl::eventRegionTriggered(EventRegionPtr region, EventType evType ) {
//LOGGING_INFO(mcLogger, "eventRegion (region type: " << region->getEventRegionType() <<
//", event Type: " << evType << ")" <<endl);
if( region->getEventRegionType() == CROSS_SECTION_BLINK )
{
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredBlinkingRegion(region) );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftBlinkingRegion() );
} else if( region->getEventRegionType() == CROSS_SECTION_HALT ) {
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredCrossingHaltRegion(region) );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftCrossingHaltRegion() );
} else if( region->getEventRegionType() == CROSS_SECTION_CENTER ) {
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredCrossingCenterRegion() );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftCrossingCenterRegion() );
} else if( region->getEventRegionType() == PARKING_SIGN ) {
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredParkingSignRegion(region) );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftParkingSignRegion() );
} else if( region->getEventRegionType() == PARKING_PARALLEL ) {
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredParkingParallelRegion() );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftParkingParallelRegion( region ) );
} else if( region->getEventRegionType() == PARKING_CROSS ) {
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredParkingCrossRegion() );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftParkingCrossRegion( region ) );
} else if(region->getEventRegionType() == OBSTACLE_REGION){
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredObstacleRegion( region ) );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftObstacleRegion(region) );
} else if(region->getEventRegionType() == OBSTACLE_REGION_SMALL){
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredObstacleRegion( region ) );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftObstacleRegion(region) );
} else if(region->getEventRegionType() == OBSTACLE_PASSED_REGION){
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredObstaclePassedRegion() );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftObstaclePassedRegion() );
} else if(region->getEventRegionType() == OVERTAKE_FINISHED_REGION){
if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvTimerObstacleRightlane() );
} else if(region->getEventRegionType() == UNCONNECTED_TRAFFIC_SIGN){
if( evType == EVENT_ENTERED_REGION )
mStateMachine.process_event( EvEnteredUnconnectedTrafficSignRegion() );
else if( evType == EVENT_EXITED_REGION )
mStateMachine.process_event( EvLeftUnconnectedTrafficSignRegion() );
} else if(region->getEventRegionType() == CROSS_SECTION_OBSTACLES){
LOGGING_WARNING( mcLogger, "Ignoring event region of CROSS_SECTION_OBSTACLES" << endl );
} else {
LOGGING_WARNING( mcLogger, "Don't know how to handle event region type:" <<
region->getEventRegionType() << "!" << endl );
}
}
