int32_t WINAPI DetourWeWinMain(HINSTANCE instance, HINSTANCE prevInstance, LPSTR commandLine, int32_t showCommand)
{
LOGGING_INFO(lg) << "Entering main program.";
LOGGING_DEBUG(lg) << "Command line: " << (commandLine ? commandLine : "NULL");
// Initialize COM
base::com::guard com;
event_array[EVENT_WE_START]([&](lua_State* L, int idx){
});
int32_t result = base::std_call<int32_t>(pgTrueWeWinMain, instance, prevInstance, commandLine, showCommand);
event_array[EVENT_WE_EXIT]([&](lua_State* L, int idx){
});
LOGGING_INFO(lg) << "Main program exit.";
// Finish engine here to prevent .net module crash
// Before we stop the lua engine, we should stop hook
// UninstallHooks();
//ShutdownLua();
return result;
}
MissionControl::MissionControl( IControl4MC* controller, DriverModule* driverModule,
TrajectoryFactory* trajectoryFactory, StreetPatcher* streetPatcher )
: mStateMachine( controller, driverModule, trajectoryFactory, streetPatcher )
, tf(trajectoryFactory)
, dm(driverModule)
, sp(streetPatcher)
{
this->controller = controller;
//hier gibt es die unterschiedlichen Debug Levels. Falls du noch einen Stream brauchst muss du ihn in der mcLogging.h und der XML definieren
//LOGGING_TRACE(mcLogger, "mcLogger Logger is running (Trace)"<<endl);
//LOGGING_DEBUG(mcLogger, "mcLogger Logger is running.(Debug)"<<endl);
LOGGING_INFO(mcLogger, "Missioncontrol constructed."<<endl);
//LOGGING_WARNING(mcLogger,"mcLogger Logger is running (Warning)"<<endl );
//LOGGING_ERROR(mcLogger, "mcLogger Logger is running (Error)"<<endl);
m_maneuverList = ManeuverList::getDummy();
LOGGING_INFO( mcLogger, "Default maneuverlist: " << m_maneuverList->toString() );
mStateMachine.setManeuverList( m_maneuverList );
mStateMachine.initiate();
// Start listening to timer events. From now on, for every timer,
// my eventTimerFired function will be calle.d
controller->getTimer()->addListener( this );
}
void MissionControl::eventManeuverListReceived( std::string maneuverList )
{
LOGGING_INFO(mcLogger, "Maneuverlist received."<<endl);
m_maneuverList = ManeuverList::parse(maneuverList, this);
//TODO fire we are ready.
mStateMachine.setManeuverList( m_maneuverList );
mStateMachine.process_event( EvGetReady() );
LOGGING_INFO( mcLogger, "ManeuverList:" << endl << m_maneuverList->toString() );
}
void MissionControl::setBoostCommand( enumSpeedCommand command )
{
if( command == SPEED_COMMAND_BOOST )
{
LOGGING_INFO( mcLogger, "Received Boost command." << endl );
mStateMachine.process_event( EvSpeedBoost() );
} else {
LOGGING_INFO( mcLogger, "Received Speed Normal command." << endl );
mStateMachine.process_event( EvSpeedNormal() );
}
}
DS301Node::Ptr DS301Group::addNode(const uint8_t node_id, const CanDevPtr can_device, HeartBeatMonitor::Ptr heartbeat_monitor)
{
LOGGING_INFO(CanOpen, "Adding new DS301Node with id " << node_id << endl);
DS301Node::Ptr node(new NodeT(node_id, can_device, heartbeat_monitor));
m_nodes.push_back(node);
return node;
}
void MissionControl::eventReadyToGeneratePatchTrajectory() {
LOGGING_INFO(mcLogger, "eventReadyToGenerateTrajectory"<<endl);
mStateMachine.process_event( EvTrajectoryReady() );
}
void MissionControl::eventTrajectoryEmpty() {
LOGGING_INFO(mcLogger, "eventTrajectoryEmpty"<<endl);
mStateMachine.process_event( EvTrajectoryEmpty() );
}
void MissionControl::eventJurySignalReceived (juryActions ja, int maneuverEntryID )
{
LOGGING_INFO(mcLogger, "eventJurySignalReceived: " << endl <<
"\tmaneuverID:" << maneuverEntryID << " action: " << ja << endl );
if(maneuverEntryID < 0) {
LOGGING_ERROR(mcLogger, "received a negative maneuver id! Cant handle this!!!!!" <<endl);
return;
}
if(((unsigned int)maneuverEntryID) != m_maneuverList->getCurrentAbsManeuverID()) {
m_maneuverList->setManeuverId(maneuverEntryID);
}
switch (ja) {
case action_GETREADY :
mStateMachine.process_event( EvGetReady() );
break;
case action_START:
//if(m_maneuverList->setManeuverId(maneuverEntryID) || true) { //TODO remove true
mStateMachine.process_event( EvJuryGo() );
/*} else {
LOGGING_ERROR(mcLogger, "Got wrong entry id!" <<endl);
mStateMachine.process_event( EvError() );
}*/
break;
case action_STOP:
//TODO: send stop to driver module, best in the SM itself
mStateMachine.process_event( EvJuryStop() );
break;
}
}
void MissionControl::eventTimerFired( timerType type, unsigned long timerID )
{
if( type == TIMER_TYPE_HALTING_AT_CROSSING )
{
LOGGING_INFO(mcLogger, "Timer fired (Halting at crossing)"<<endl);
mStateMachine.process_event( EvTimerHaltingAtCrossing() );
} else if( type == TIMER_TYPE_PARKING )
{
LOGGING_INFO(mcLogger, "Timer fired (parking)" <<endl);
mStateMachine.process_event( EvTimerParking() );
} else if( type == TIMER_TYPE_HALTING_AT_OBSTACLE )
{
LOGGING_INFO(mcLogger, "Timer fired (obstacle stop)" <<endl);
mStateMachine.process_event( EvTimerObstacleStop() );
} else if( type == TIMER_TYPE_CHECKING_AT_OBSTACLE )
{
LOGGING_INFO(mcLogger, "Timer fired (obstacle check)" <<endl);
mStateMachine.process_event( EvTimerObstacleCheck() );
} else if( type == TIMER_TYPE_RETURN_AFTER_OBSTACLE )
{
LOGGING_INFO(mcLogger, "Timer fired (return after obstacle)" <<endl);
mStateMachine.process_event( EvTimerObstacleRightlane() );
} else if( type == TIMER_TYPE_CHECK_TRAJECTORY_FREE )
{
LOGGING_INFO(mcLogger, "Timer fired (check if return traj is free)" <<endl);
mStateMachine.process_event( EvTimerTrajectoryFree() );
} else if( type == TIMER_TYPE_CHECK_TRAJECTORY_FREE_AGAIN )
{
LOGGING_INFO(mcLogger, "Timer fired (check if return traj is free...again)" <<endl);
mStateMachine.process_event( EvTimerTrajectoryFreeAgain() );
} else if( type == TIMER_TYPE_DRIVE_WITHOUT_TRAJECTORY )
{
LOGGING_INFO(mcLogger, "Timer fired ( drive without a trajetory)" <<endl);
mStateMachine.process_event( EvTimerDriveWithNoTrajectory() );
} else if( type == TIMER_SET_SMALL_FORWARD_TRAJ )
{
LOGGING_INFO(mcLogger, "Timer fired ( setting small forward trajetory)" <<endl);
mStateMachine.process_event( EvTimerSetSmallForwardTraj() );
} else {
// ignore
}
}
int main(int argc, char* argv1[])
{
TSK_VS_INFO* lVsInfo = NULL;
TSK_OFF_T lCnt = 0;
char lBuf[32768] = { 0 };
unsigned lCntRead = 0;
TSK_IMG_INFO* lImgInfo = OS_FH_INVALID;
OS_FH_TYPE lOut = OS_FH_INVALID;
const TSK_TCHAR *const *argv;
#ifdef TSK_WIN32
argv = CommandLineToArgvW(GetCommandLineW(), &argc);
#else
argv = (const TSK_TCHAR *const *) argv1;
#endif
lOut = OS_FOPEN_WRITE(argv[2]);
if (lOut == OS_FH_INVALID)
{
LOGGING_ERROR("Could not open export image in write mode. \n")
exit(1);
}
lImgInfo = tsk_img_open(
1, /* number of images */
(argv + 1), /* path to images */
TSK_IMG_TYPE_DETECT, /* disk image type */
0); /* size of device sector in bytes */
if (lImgInfo != NULL)
{
TSK_OFF_T lSizeSectors = lImgInfo->size / lImgInfo->sector_size + \
(lImgInfo->size % lImgInfo->sector_size ? 1 : 0);
LOGGING_INFO("Image size (Bytes): %lu, Image size (sectors): %lu\n",
lImgInfo->size,
lSizeSectors);
lVsInfo = tsk_vs_open(lImgInfo, 0, TSK_VS_TYPE_DETECT);
if (lVsInfo != NULL)
{
if (tsk_vs_part_walk(lVsInfo,
0, /* start */
lVsInfo->part_count - 1, /* end */
TSK_VS_PART_FLAG_ALL, /* all partitions */
part_act, /* callback */
(void*) lOut /* data passed to the callback */
) != 0)
{
fprintf(stderr, "Problem when walking partitions. \n");
}
}
else
{
LOGGING_DEBUG("Volume system cannot be opened.\n");
for (lCnt = 0; lCnt < lSizeSectors; lCnt++)
{
lCntRead = lCnt == lSizeSectors - 1 ?
lImgInfo->size % lImgInfo->sector_size :
lImgInfo->sector_size;
LOGGING_DEBUG("Reading %u bytes\n", lCntRead);
tsk_img_read(
lImgInfo, /* handler */
lCnt * lImgInfo->sector_size, /* start address */
lBuf, /* buffer to store data in */
lCntRead /* amount of data to read */
);
data_act(lBuf, lCntRead, lCnt * lImgInfo->sector_size, lOut);
}
}
}
else
{
LOGGING_ERROR("Problem opening the image. \n");
tsk_error_print(stderr);
exit(1);
}
if (lOut != OS_FH_INVALID)
{
OS_FCLOSE(lOut);
}
return EXIT_SUCCESS;
}
void __fastcall FakeWeMessageShow(const char *message, int flag)
{
LOGGING_INFO(lg) << message;
base::fast_call<void>(RealWeMessageShow, message, flag);
}
void TrafficSignDetAruco::detectMarkers(cv::Mat& input, bool histogramEqualization /*= true*/) {
// only perform detection if dictionary is loaded, otherwise aruco's detection crashes because of a division by zero
if(dictionaryLoaded) {
// next line not needed, it is performed in aruco's detect method
//detectedMarkers.clear();
cv::Mat equalizedImage(input.size(), input.type());
cv::Mat* image;
image = &input;
// detect the marker
markerDetector.detect(*image, detectedMarkers, cameraMatrix, distortionCoefficients, MARKER_SIZE);
if(detectedMarkers.size() > 0) {
// save current car pose
oadrive::core::ExtendedPose2d positionCar( Environment::getInstance()->getCarPose() );
// calculate position of markers in camera coordinate system and add every marker to environment
for(size_t i = 0; i < detectedMarkers.size(); ++i) {
// map traffic sign into range [0, 2*Pi)
cv::Mat R;
cv::Rodrigues(detectedMarkers[i].Rvec, R); // R is 3x3
double r32 = R.at<float>(2,1);
double r33 = R.at<float>(2,2);
double trafficSignYaw = std::atan2(r32,r33);
trafficSignYaw *= -1;
int factor = trafficSignYaw / (2 * M_PI);
trafficSignYaw -= factor * 2 * M_PI;
if(trafficSignYaw < 0) {
trafficSignYaw += 2 * M_PI;
}
// angle must be in range (Pi/2 to 3*Pi/2) - otherwise the car can't see the sign
/*if(trafficSignYaw < M_PI_2) {
trafficSignYaw += M_PI;
}
else if(trafficSignYaw > M_PI + M_PI_2) {
trafficSignYaw -= M_PI;
}*/
//std::cout << "yaw: " << trafficSignYaw << std::endl;
oadrive::core::ExtendedPose2d positionMarkerCar(detectedMarkers[i].Tvec.at<float>(0) + CAMERA2CAR_X, detectedMarkers[i].Tvec.at<float>(2) + CAMERA2CAR_Y, trafficSignYaw);
oadrive::core::ExtendedPose2d positionWorld( birdViewPositionConverter->car2World(positionCar, positionMarkerCar) );
LOGGING_INFO(TrafficSignLogger, "traffic sign detected: " << detectedMarkers[i].id << ", x: " << detectedMarkers[i].Tvec.at<float>(0) + CAMERA2CAR_X << ", y: " << detectedMarkers[i].Tvec.at<float>(2) + CAMERA2CAR_Y << ", yaw: " << trafficSignYaw << endl);
Environment::getInstance()->addTrafficSign(positionWorld, detectedMarkers[i].id);
}
}
// histogram equalization
cv::equalizeHist(input, equalizedImage);
image = &equalizedImage;
// detect the marker
markerDetector.detect(*image, detectedMarkers, cameraMatrix, distortionCoefficients, MARKER_SIZE);
if(detectedMarkers.size() > 0) {
// save current car pose
oadrive::core::ExtendedPose2d positionCar( Environment::getInstance()->getCarPose() );
// calculate position of markers in camera coordinate system and add every marker to environment
for(size_t i = 0; i < detectedMarkers.size(); ++i) {
// map traffic sign into range [0, 2*Pi)
cv::Mat R;
cv::Rodrigues(detectedMarkers[i].Rvec, R); // R is 3x3
double r32 = R.at<float>(2,1);
double r33 = R.at<float>(2,2);
double trafficSignYaw = std::atan2(r32,r33);
trafficSignYaw *= -1;
int factor = trafficSignYaw / (2 * M_PI);
trafficSignYaw -= factor * 2 * M_PI;
if(trafficSignYaw < 0) {
trafficSignYaw += 2 * M_PI;
}
// angle must be in range (Pi/2 to 3*Pi/2) - otherwise the car can't see the sign
/*if(trafficSignYaw < M_PI_2) {
trafficSignYaw += M_PI;
}
else if(trafficSignYaw > M_PI + M_PI_2) {
trafficSignYaw -= M_PI;
}*/
//std::cout << "yaw: " << trafficSignYaw << std::endl;
oadrive::core::ExtendedPose2d positionMarkerCar(detectedMarkers[i].Tvec.at<float>(0) + CAMERA2CAR_X, detectedMarkers[i].Tvec.at<float>(2) + CAMERA2CAR_Y, trafficSignYaw);
oadrive::core::ExtendedPose2d positionWorld( birdViewPositionConverter->car2World(positionCar, positionMarkerCar) );
LOGGING_INFO(TrafficSignLogger, "traffic sign detected: " << detectedMarkers[i].id << ", x: " << detectedMarkers[i].Tvec.at<float>(0) + CAMERA2CAR_X << ", y: " << detectedMarkers[i].Tvec.at<float>(2) + CAMERA2CAR_Y << ", yaw: " << trafficSignYaw << endl);
Environment::getInstance()->addTrafficSign(positionWorld, detectedMarkers[i].id);
}
}
}
}
int luaopen_wehelper(lua_State* L) {
NYDWE::lg = logging::get(L);
NYDWE::InstallHooks();
LOGGING_INFO(NYDWE::lg) << "YDWE startup complete.";
return 0;
}
void MissionControl::eventUTurn() {
LOGGING_INFO( mcLogger, "Received U-Turn command." << endl );
mStateMachine.process_event( EvUTurn() );
}
//TODO: remove
void MissionControl::eventWaitOver() {
LOGGING_INFO(mcLogger, "eventWaitOver"<<endl);
//dm->drive();
}
void IbeoSourceWrapper::run()
{
// Create ibeo source
icl_sourcesink::SourceSinkManager manager;
nibeo::IbeoSourceNoAPI::Ptr ibeo_source = manager.createSource<nibeo::IbeoMsg>(m_ibeo_uri, std::vector<std::string>(), true);
nncom::NcomSource::Ptr ncom_source = manager.createSource<nncom::Ncom>(m_ncom_uri);
// We create a data element ptr that we will use to work on.
nibeo::IbeoMsgStamped::Ptr ibeo_element;
nncom::NcomStamped::Ptr ncom_element;
icl_gps::AbsolutePose* ref_pose = NULL;
// for each time step
for (; manager.good(); manager.advance())
{
ibeo_element = ibeo_source->get();
ncom_element = ncom_source->get();
// Skip messages other than IbeoScanMsg
nibeo::IbeoScanMsg scan_msg;
if(!scan_msg.fromIbeoMsg(*ibeo_element))
continue;
LOGGING_INFO(Gpu_voxels, "Number of points in ibeo msg: " << scan_msg.number_of_points << endl);
// Copy points to std::vector
std::vector<Vector3f> point_cloud(scan_msg.number_of_points);
for(int i = 0; i < scan_msg.number_of_points; ++i)
{
point_cloud[i].x = (*scan_msg.scan_points)[i].x;
point_cloud[i].y = (*scan_msg.scan_points)[i].y;
point_cloud[i].z = (*scan_msg.scan_points)[i].z;
}
// Handle position
nncom::Ncom ncom_msg = *ncom_element; // Unwrap message
icl_gps::AbsolutePose abs_pose(ncom_msg->mLon, ncom_msg->mLat, ncom_msg->mAlt, ncom_msg->mTrack);
LOGGING_INFO(Gpu_voxels, "Current pose. long: " << abs_pose.longitude() << " lat: " << abs_pose.latitude() << " alt: " << abs_pose.altitude() << " bear: " << abs_pose.bearing() << endl);
if(!ref_pose) // Use first position as reference
{
ref_pose = new icl_gps::AbsolutePose(abs_pose.longitude(), abs_pose.latitude(), abs_pose.altitude(), abs_pose.bearing());
LOGGING_INFO(Gpu_voxels, "Set reference pose. long: " << abs_pose.longitude() << " lat: " << abs_pose.latitude() << " alt: " << abs_pose.altitude() << " bear: " << abs_pose.bearing() << endl);
}
icl_gps::RelativePose rel_pose = abs_pose - *ref_pose;
icl_math::Pose2d rel_pose2d = rel_pose.toPose2d();
LOGGING_INFO(Gpu_voxels, "Relative pose. north: " << rel_pose.north() << " east: " << rel_pose.east() << " alt: " << rel_pose.altitude() << endl);
Matrix4f matrix;
// copy pose2d rotation matrix
matrix.a11 = rel_pose2d(0,0); matrix.a12 = rel_pose2d(0,1);
matrix.a21 = rel_pose2d(1,0); matrix.a22 = rel_pose2d(1,1);
// copy pose2d translation vector
matrix.a14 = rel_pose2d(0,2);
matrix.a24 = rel_pose2d(1,2);
// set generals structure of transformation matrix
matrix.a33 = 1.0f;
matrix.a44 = 1.0f;
// set z-translation (altitude)
matrix.a34 = static_cast<float>(rel_pose.altitude());
// set additional translation
matrix.a14 += m_additional_translation.x;
matrix.a24 += m_additional_translation.y;
matrix.a34 += m_additional_translation.z;
LOGGING_INFO(Gpu_voxels, "Matrix4f: " << endl << matrix.a11 << " " << matrix.a12 << " " << matrix.a13 << " " << matrix.a14 << endl
<< matrix.a21 << " " << matrix.a22 << " " << matrix.a23 << " " << matrix.a24 << endl
<< matrix.a31 << " " << matrix.a32 << " " << matrix.a33 << " " << matrix.a34 << endl
<< matrix.a41 << " " << matrix.a42 << " " << matrix.a43 << " " << matrix.a44 << endl);
LOGGING_INFO(Gpu_voxels, "Transform point cloud..." << endl);
CudaMath::transform(point_cloud, matrix); // todo: keep point cloud in gpu mem
m_callback(point_cloud);
}
}
