Example #1
0
	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;
	}
Example #2
0
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 );
}
Example #3
0
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() );
}
Example #4
0
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;
}
Example #6
0
void MissionControl::eventReadyToGeneratePatchTrajectory() {
  LOGGING_INFO(mcLogger, "eventReadyToGenerateTrajectory"<<endl);

  mStateMachine.process_event( EvTrajectoryReady() );


}
Example #7
0
void MissionControl::eventTrajectoryEmpty() {

  LOGGING_INFO(mcLogger, "eventTrajectoryEmpty"<<endl);

  mStateMachine.process_event( EvTrajectoryEmpty() );

}
Example #8
0
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;
  }
}
Example #9
0
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
  }
}
Example #10
0
File: tsk_test.c Project: Jdev1/mmc
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;
}
Example #11
0
	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);
      }
    }
  }
}
Example #13
0
int luaopen_wehelper(lua_State* L) {
	NYDWE::lg = logging::get(L);
	NYDWE::InstallHooks();
	LOGGING_INFO(NYDWE::lg) << "YDWE startup complete.";
	return 0;
}
Example #14
0
void MissionControl::eventUTurn() {
  LOGGING_INFO( mcLogger, "Received U-Turn command." << endl );
  mStateMachine.process_event( EvUTurn() );
}
Example #15
0
//TODO: remove
void MissionControl::eventWaitOver() {
  LOGGING_INFO(mcLogger, "eventWaitOver"<<endl);

  //dm->drive();
}
Example #16
0
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);
    }
}