├── .gitignore ├── .vscode ├── c_cpp_properties.json ├── launch.json └── settings.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── FindEigen3.cmake ├── data ├── AprilTags │ ├── Tag16h5.pdf │ ├── Tag25h9.pdf │ └── Tag36h11.pdf ├── LandingPad │ ├── ArucoPad_Tag16h5.ai │ └── LandingPad_16h5x6p.pdf ├── Livox │ ├── HAP_config.json │ ├── mid360_200.json │ ├── mid360_201.json │ ├── mid360_202.json │ ├── mid360_203.json │ ├── mid360_204.json │ ├── mid360_8.json │ ├── mid360_8_103.json │ └── mid360_config.json ├── OK.png ├── calibration │ ├── circle.png │ └── pattern.png ├── exiftool │ └── example.config ├── model │ └── synset_words.txt └── orb_slam │ ├── TUM1.yaml │ ├── logicool_brio_1280x720.yaml │ └── logicool_brio_640x360.yaml ├── docs └── setup.md ├── firmware └── arduino │ ├── arduCanbus.ino │ ├── arduEtherServo.ino │ ├── arduGPSbeacon.ino │ ├── arduLED.ino │ ├── arduPWM2BLC.ino │ ├── arduPWM2Lev.ino │ ├── arduPWMio.ino │ ├── arduRover.ino │ ├── arduSBUS.ino │ ├── arduSBUSuart.ino │ ├── arduServo.ino │ └── include │ ├── BLC.h │ ├── arduMotorBLC.h │ ├── arduMotorPWM.h │ ├── arduOK.h │ ├── arduPPMreader.h │ └── arduUDP.h ├── kiss ├── Open3D.kiss ├── OpenCL.kiss ├── RPi_video.kiss ├── TGRS.kiss ├── _ADIO_EBYTE.kiss ├── _AP_actuator.kiss ├── _ArUco.kiss ├── _ArduServo.kiss ├── _BenewakeTF03.kiss ├── _CamCalib.kiss ├── _Camera.kiss ├── _Chilitags.kiss ├── _DDSM.kiss ├── _DNNclassifier.kiss ├── _DNNtext.kiss ├── _DenseFlow.kiss ├── _Detectors.kiss ├── _Feetech.kiss ├── _GDimgUploader.kiss ├── _GPS.kiss ├── _GeometryViewer.kiss ├── _HYMCU_RS485.kiss ├── _HandKey.kiss ├── _ImgFilter.kiss ├── _InfiRay.kiss ├── _Lane.kiss ├── _LeddarVu.kiss ├── _Livox2.kiss ├── _Mavlink.kiss ├── _MotionDetector.kiss ├── _ORB_SLAM.kiss ├── _OpenPose.kiss ├── _OrientalMotor.kiss ├── _PCgrid.kiss ├── _PCregistGlobal.kiss ├── _ROS_fastLio.kiss ├── _RealSense.kiss ├── _SharedMemImg.kiss ├── _TOFsense.kiss ├── _TensorFlowLite.kiss ├── _Thermal.kiss ├── _Uploader.kiss ├── _Vzense.kiss ├── _WebSocket.kiss ├── _XDynamics.kiss ├── _Xbee.kiss ├── _ZDmotor.kiss ├── _ZLAC8015.kiss ├── _xArm.kiss ├── apCopter.kiss ├── apCopter_fastLio.kiss ├── apMavlinkGphoto.kiss ├── mavlinkRouter.kiss └── test │ ├── JsonBaseUDP.kiss │ ├── WebSocket.kiss │ └── apMavlinkFollow.kiss ├── package.xml ├── sh ├── Config │ ├── VzSetup.sh │ ├── config_optimize.sh │ └── usbReset.sh ├── Deploy │ └── gsvLidars.sh ├── Gstreamer │ ├── gst.sh │ └── gstSend.sh ├── Jetson │ ├── HZHY.sh │ ├── ok_jetson.sh │ └── setBoot │ │ ├── NVMeBoot │ │ ├── LICENSE │ │ ├── README.md │ │ ├── copy-rootfs-ssd.sh │ │ ├── data │ │ │ ├── setssdroot.service │ │ │ └── setssdroot.sh │ │ └── setup-service.sh │ │ ├── SDboot │ │ ├── LICENSE │ │ ├── README.md │ │ ├── copy-rootfs-tf.sh │ │ ├── data │ │ │ ├── setssdroot.service │ │ │ └── setssdroot.sh │ │ └── setup-service.sh │ │ └── auto_partition.sh ├── RaspberryPi │ ├── CM4_dt_blob.7z │ ├── INA219.py │ ├── ok.service │ └── ok_rpi.sh └── Setup │ ├── ardupilot_dev_pc.sh │ ├── fast_lio_common.sh │ ├── open3D.sh │ ├── openCL.sh │ ├── openCV_RPi.sh │ ├── openCV_jetson.sh │ ├── openCV_pc.sh │ ├── openCV_pc_cuda.sh │ ├── openSplat.sh │ ├── ros2_common.sh │ └── setup_common.sh ├── src ├── 3D │ ├── Mesh │ │ ├── _MeshStream.cpp │ │ └── _MeshStream.h │ ├── PointCloud │ │ ├── PCfilter │ │ │ ├── _PCcrop.cpp │ │ │ ├── _PCcrop.h │ │ │ ├── _PCdownSample.cpp │ │ │ ├── _PCdownSample.h │ │ │ ├── _PCremove.cpp │ │ │ ├── _PCremove.h │ │ │ ├── _PCtransform.cpp │ │ │ └── _PCtransform.h │ │ ├── PCregistration │ │ │ ├── _PCregistCol.cpp │ │ │ ├── _PCregistCol.h │ │ │ ├── _PCregistGlobal.cpp │ │ │ ├── _PCregistGlobal.h │ │ │ ├── _PCregistICP.cpp │ │ │ └── _PCregistICP.h │ │ ├── _PCfile.cpp │ │ ├── _PCfile.h │ │ ├── _PCframe.cpp │ │ ├── _PCframe.h │ │ ├── _PCgrid.cpp │ │ ├── _PCgrid.h │ │ ├── _PCmerge.cpp │ │ ├── _PCmerge.h │ │ ├── _PCrecv.cpp │ │ ├── _PCrecv.h │ │ ├── _PCsend.cpp │ │ ├── _PCsend.h │ │ ├── _PCstream.cpp │ │ └── _PCstream.h │ ├── _GeometryBase.cpp │ ├── _GeometryBase.h │ ├── _GeometryViewer.cpp │ └── _GeometryViewer.h ├── Actuator │ ├── Articulated │ │ ├── _xArm.cpp │ │ └── _xArm.h │ ├── Motor │ │ ├── _DDSM.cpp │ │ ├── _DDSM.h │ │ ├── _ZDmotor.cpp │ │ ├── _ZDmotor.h │ │ ├── _ZLAC8015.cpp │ │ ├── _ZLAC8015.h │ │ ├── _ZLAC8015D.cpp │ │ └── _ZLAC8015D.h │ ├── _ActuatorBase.cpp │ ├── _ActuatorBase.h │ ├── _ArduServo.cpp │ ├── _ArduServo.h │ ├── _Feetech.cpp │ ├── _Feetech.h │ ├── _HYMCU_RS485.cpp │ ├── _HYMCU_RS485.h │ ├── _MultiActuatorsBase.cpp │ ├── _MultiActuatorsBase.h │ ├── _OrientalMotor.cpp │ └── _OrientalMotor.h ├── Arithmetic │ ├── Destimator.cpp │ ├── Destimator.h │ ├── PolyFit.cpp │ └── PolyFit.h ├── Autopilot │ ├── APmavlink │ │ ├── _APmavlink_GPS.cpp │ │ ├── _APmavlink_GPS.h │ │ ├── _APmavlink_avoid.cpp │ │ ├── _APmavlink_avoid.h │ │ ├── _APmavlink_base.cpp │ │ ├── _APmavlink_base.h │ │ ├── _APmavlink_depthVision.cpp │ │ ├── _APmavlink_depthVision.h │ │ ├── _APmavlink_distLidar.cpp │ │ ├── _APmavlink_distLidar.h │ │ ├── _APmavlink_drive.cpp │ │ ├── _APmavlink_drive.h │ │ ├── _APmavlink_follow.cpp │ │ ├── _APmavlink_follow.h │ │ ├── _APmavlink_httpJson.cpp │ │ ├── _APmavlink_httpJson.h │ │ ├── _APmavlink_land.cpp │ │ ├── _APmavlink_land.h │ │ ├── _APmavlink_landingTarget.cpp │ │ ├── _APmavlink_landingTarget.h │ │ ├── _APmavlink_mav2json.cpp │ │ ├── _APmavlink_mav2json.h │ │ ├── _APmavlink_mission.cpp │ │ ├── _APmavlink_mission.h │ │ ├── _APmavlink_move.cpp │ │ ├── _APmavlink_move.h │ │ ├── _APmavlink_photo.cpp │ │ ├── _APmavlink_photo.h │ │ ├── _APmavlink_rcChannel.cpp │ │ ├── _APmavlink_rcChannel.h │ │ ├── _APmavlink_relay.cpp │ │ ├── _APmavlink_relay.h │ │ ├── _APmavlink_servo.cpp │ │ ├── _APmavlink_servo.h │ │ ├── _APmavlink_swarm.cpp │ │ ├── _APmavlink_swarm.h │ │ ├── _APmavlink_video.cpp │ │ ├── _APmavlink_video.h │ │ ├── _APmavlink_videoStream.cpp │ │ ├── _APmavlink_videoStream.h │ │ ├── _APmavlink_visionEstimate.cpp │ │ └── _APmavlink_visionEstimate.h │ └── Drive │ │ ├── _Drive.cpp │ │ └── _Drive.h ├── Base │ ├── BASE.cpp │ ├── BASE.h │ ├── _ModuleBase.cpp │ ├── _ModuleBase.h │ ├── _Thread.cpp │ ├── _Thread.h │ ├── common.h │ ├── constant.h │ ├── cv.h │ ├── macro.h │ ├── open3d.h │ └── platform.h ├── Compute │ └── OpenCL │ │ ├── clBase.cpp │ │ └── clBase.h ├── Control │ ├── PID.cpp │ └── PID.h ├── DNN │ ├── JetsonInference │ │ ├── _DetectNet.cpp │ │ └── _DetectNet.h │ └── TensorFlowLite │ │ ├── _TFmobileNet.cpp │ │ └── _TFmobileNet.h ├── Data │ ├── Image │ │ ├── _BBoxCutOut.cpp │ │ ├── _BBoxCutOut.h │ │ ├── _CutOut.cpp │ │ ├── _CutOut.h │ │ ├── _GDimgUploader.cpp │ │ ├── _GDimgUploader.h │ │ ├── _GPhotoTake.cpp │ │ ├── _GPhotoTake.h │ │ ├── _PhotoTake.cpp │ │ └── _PhotoTake.h │ ├── _FileBase.cpp │ └── _FileBase.h ├── Dependencies │ ├── CRC.h │ ├── Feetech │ │ ├── INST.h │ │ ├── SCS.cpp │ │ ├── SCS.h │ │ ├── SCSCL.cpp │ │ ├── SCSCL.h │ │ ├── SCSerial.cpp │ │ ├── SCSerial.h │ │ ├── SCServo.h │ │ ├── SMSBL.cpp │ │ ├── SMSBL.h │ │ ├── SMSCL.cpp │ │ ├── SMSCL.h │ │ ├── SMS_STS.cpp │ │ └── SMS_STS.h │ ├── SensorFusion │ │ ├── SensorFusion.cpp │ │ └── SensorFusion.h │ ├── UTM.h │ ├── c_library_v2 │ │ ├── ASLUAV │ │ │ ├── ASLUAV.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_asl_obctrl.h │ │ │ ├── mavlink_msg_aslctrl_data.h │ │ │ ├── mavlink_msg_aslctrl_debug.h │ │ │ ├── mavlink_msg_asluav_status.h │ │ │ ├── mavlink_msg_command_int_stamped.h │ │ │ ├── mavlink_msg_command_long_stamped.h │ │ │ ├── mavlink_msg_ekf_ext.h │ │ │ ├── mavlink_msg_fw_soaring_data.h │ │ │ ├── mavlink_msg_gsm_link_status.h │ │ │ ├── mavlink_msg_satcom_link_status.h │ │ │ ├── mavlink_msg_sens_atmos.h │ │ │ ├── mavlink_msg_sens_batmon.h │ │ │ ├── mavlink_msg_sens_mppt.h │ │ │ ├── mavlink_msg_sens_power.h │ │ │ ├── mavlink_msg_sens_power_board.h │ │ │ ├── mavlink_msg_sensor_airflow_angles.h │ │ │ ├── mavlink_msg_sensorpod_status.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── AVSSUAS │ │ │ ├── AVSSUAS.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_avss_drone_imu.h │ │ │ ├── mavlink_msg_avss_drone_operation_mode.h │ │ │ ├── mavlink_msg_avss_drone_position.h │ │ │ ├── mavlink_msg_avss_prs_sys_status.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── all │ │ │ ├── all.h │ │ │ ├── mavlink.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── ardupilotmega │ │ │ ├── ardupilotmega.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_adap_tuning.h │ │ │ ├── mavlink_msg_ahrs.h │ │ │ ├── mavlink_msg_ahrs2.h │ │ │ ├── mavlink_msg_ahrs3.h │ │ │ ├── mavlink_msg_airspeed_autocal.h │ │ │ ├── mavlink_msg_aoa_ssa.h │ │ │ ├── mavlink_msg_ap_adc.h │ │ │ ├── mavlink_msg_autopilot_version_request.h │ │ │ ├── mavlink_msg_battery2.h │ │ │ ├── mavlink_msg_camera_feedback.h │ │ │ ├── mavlink_msg_camera_status.h │ │ │ ├── mavlink_msg_compassmot_status.h │ │ │ ├── mavlink_msg_data16.h │ │ │ ├── mavlink_msg_data32.h │ │ │ ├── mavlink_msg_data64.h │ │ │ ├── mavlink_msg_data96.h │ │ │ ├── mavlink_msg_deepstall.h │ │ │ ├── mavlink_msg_device_op_read.h │ │ │ ├── mavlink_msg_device_op_read_reply.h │ │ │ ├── mavlink_msg_device_op_write.h │ │ │ ├── mavlink_msg_device_op_write_reply.h │ │ │ ├── mavlink_msg_digicam_configure.h │ │ │ ├── mavlink_msg_digicam_control.h │ │ │ ├── mavlink_msg_ekf_status_report.h │ │ │ ├── mavlink_msg_esc_telemetry_1_to_4.h │ │ │ ├── mavlink_msg_esc_telemetry_5_to_8.h │ │ │ ├── mavlink_msg_esc_telemetry_9_to_12.h │ │ │ ├── mavlink_msg_fence_fetch_point.h │ │ │ ├── mavlink_msg_fence_point.h │ │ │ ├── mavlink_msg_gimbal_control.h │ │ │ ├── mavlink_msg_gimbal_report.h │ │ │ ├── mavlink_msg_gimbal_torque_cmd_report.h │ │ │ ├── mavlink_msg_gopro_get_request.h │ │ │ ├── mavlink_msg_gopro_get_response.h │ │ │ ├── mavlink_msg_gopro_heartbeat.h │ │ │ ├── mavlink_msg_gopro_set_request.h │ │ │ ├── mavlink_msg_gopro_set_response.h │ │ │ ├── mavlink_msg_hwstatus.h │ │ │ ├── mavlink_msg_led_control.h │ │ │ ├── mavlink_msg_limits_status.h │ │ │ ├── mavlink_msg_mag_cal_progress.h │ │ │ ├── mavlink_msg_mcu_status.h │ │ │ ├── mavlink_msg_meminfo.h │ │ │ ├── mavlink_msg_mount_configure.h │ │ │ ├── mavlink_msg_mount_control.h │ │ │ ├── mavlink_msg_mount_status.h │ │ │ ├── mavlink_msg_obstacle_distance_3d.h │ │ │ ├── mavlink_msg_osd_param_config.h │ │ │ ├── mavlink_msg_osd_param_config_reply.h │ │ │ ├── mavlink_msg_osd_param_show_config.h │ │ │ ├── mavlink_msg_osd_param_show_config_reply.h │ │ │ ├── mavlink_msg_pid_tuning.h │ │ │ ├── mavlink_msg_radio.h │ │ │ ├── mavlink_msg_rally_fetch_point.h │ │ │ ├── mavlink_msg_rally_point.h │ │ │ ├── mavlink_msg_rangefinder.h │ │ │ ├── mavlink_msg_remote_log_block_status.h │ │ │ ├── mavlink_msg_remote_log_data_block.h │ │ │ ├── mavlink_msg_rpm.h │ │ │ ├── mavlink_msg_sensor_offsets.h │ │ │ ├── mavlink_msg_set_mag_offsets.h │ │ │ ├── mavlink_msg_simstate.h │ │ │ ├── mavlink_msg_vision_position_delta.h │ │ │ ├── mavlink_msg_water_depth.h │ │ │ ├── mavlink_msg_wind.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── checksum.h │ │ ├── common │ │ │ ├── common.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_actuator_control_target.h │ │ │ ├── mavlink_msg_actuator_output_status.h │ │ │ ├── mavlink_msg_adsb_vehicle.h │ │ │ ├── mavlink_msg_ais_vessel.h │ │ │ ├── mavlink_msg_altitude.h │ │ │ ├── mavlink_msg_att_pos_mocap.h │ │ │ ├── mavlink_msg_attitude.h │ │ │ ├── mavlink_msg_attitude_quaternion.h │ │ │ ├── mavlink_msg_attitude_quaternion_cov.h │ │ │ ├── mavlink_msg_attitude_target.h │ │ │ ├── mavlink_msg_auth_key.h │ │ │ ├── mavlink_msg_autopilot_state_for_gimbal_device.h │ │ │ ├── mavlink_msg_autopilot_version.h │ │ │ ├── mavlink_msg_battery_info.h │ │ │ ├── mavlink_msg_battery_status.h │ │ │ ├── mavlink_msg_button_change.h │ │ │ ├── mavlink_msg_camera_capture_status.h │ │ │ ├── mavlink_msg_camera_fov_status.h │ │ │ ├── mavlink_msg_camera_image_captured.h │ │ │ ├── mavlink_msg_camera_information.h │ │ │ ├── mavlink_msg_camera_settings.h │ │ │ ├── mavlink_msg_camera_tracking_geo_status.h │ │ │ ├── mavlink_msg_camera_tracking_image_status.h │ │ │ ├── mavlink_msg_camera_trigger.h │ │ │ ├── mavlink_msg_can_filter_modify.h │ │ │ ├── mavlink_msg_can_frame.h │ │ │ ├── mavlink_msg_canfd_frame.h │ │ │ ├── mavlink_msg_cellular_config.h │ │ │ ├── mavlink_msg_cellular_status.h │ │ │ ├── mavlink_msg_change_operator_control.h │ │ │ ├── mavlink_msg_change_operator_control_ack.h │ │ │ ├── mavlink_msg_collision.h │ │ │ ├── mavlink_msg_command_ack.h │ │ │ ├── mavlink_msg_command_cancel.h │ │ │ ├── mavlink_msg_command_int.h │ │ │ ├── mavlink_msg_command_long.h │ │ │ ├── mavlink_msg_component_information.h │ │ │ ├── mavlink_msg_component_information_basic.h │ │ │ ├── mavlink_msg_component_metadata.h │ │ │ ├── mavlink_msg_control_system_state.h │ │ │ ├── mavlink_msg_current_event_sequence.h │ │ │ ├── mavlink_msg_data_stream.h │ │ │ ├── mavlink_msg_data_transmission_handshake.h │ │ │ ├── mavlink_msg_debug.h │ │ │ ├── mavlink_msg_debug_float_array.h │ │ │ ├── mavlink_msg_debug_vect.h │ │ │ ├── mavlink_msg_distance_sensor.h │ │ │ ├── mavlink_msg_efi_status.h │ │ │ ├── mavlink_msg_encapsulated_data.h │ │ │ ├── mavlink_msg_esc_info.h │ │ │ ├── mavlink_msg_esc_status.h │ │ │ ├── mavlink_msg_estimator_status.h │ │ │ ├── mavlink_msg_event.h │ │ │ ├── mavlink_msg_extended_sys_state.h │ │ │ ├── mavlink_msg_fence_status.h │ │ │ ├── mavlink_msg_file_transfer_protocol.h │ │ │ ├── mavlink_msg_flight_information.h │ │ │ ├── mavlink_msg_follow_target.h │ │ │ ├── mavlink_msg_generator_status.h │ │ │ ├── mavlink_msg_gimbal_device_attitude_status.h │ │ │ ├── mavlink_msg_gimbal_device_information.h │ │ │ ├── mavlink_msg_gimbal_device_set_attitude.h │ │ │ ├── mavlink_msg_gimbal_manager_information.h │ │ │ ├── mavlink_msg_gimbal_manager_set_attitude.h │ │ │ ├── mavlink_msg_gimbal_manager_set_manual_control.h │ │ │ ├── mavlink_msg_gimbal_manager_set_pitchyaw.h │ │ │ ├── mavlink_msg_gimbal_manager_status.h │ │ │ ├── mavlink_msg_global_position_int.h │ │ │ ├── mavlink_msg_global_position_int_cov.h │ │ │ ├── mavlink_msg_global_vision_position_estimate.h │ │ │ ├── mavlink_msg_gps2_raw.h │ │ │ ├── mavlink_msg_gps2_rtk.h │ │ │ ├── mavlink_msg_gps_global_origin.h │ │ │ ├── mavlink_msg_gps_inject_data.h │ │ │ ├── mavlink_msg_gps_input.h │ │ │ ├── mavlink_msg_gps_raw_int.h │ │ │ ├── mavlink_msg_gps_rtcm_data.h │ │ │ ├── mavlink_msg_gps_rtk.h │ │ │ ├── mavlink_msg_gps_status.h │ │ │ ├── mavlink_msg_high_latency.h │ │ │ ├── mavlink_msg_high_latency2.h │ │ │ ├── mavlink_msg_highres_imu.h │ │ │ ├── mavlink_msg_hil_actuator_controls.h │ │ │ ├── mavlink_msg_hil_controls.h │ │ │ ├── mavlink_msg_hil_gps.h │ │ │ ├── mavlink_msg_hil_optical_flow.h │ │ │ ├── mavlink_msg_hil_rc_inputs_raw.h │ │ │ ├── mavlink_msg_hil_sensor.h │ │ │ ├── mavlink_msg_hil_state.h │ │ │ ├── mavlink_msg_hil_state_quaternion.h │ │ │ ├── mavlink_msg_home_position.h │ │ │ ├── mavlink_msg_hygrometer_sensor.h │ │ │ ├── mavlink_msg_illuminator_status.h │ │ │ ├── mavlink_msg_isbd_link_status.h │ │ │ ├── mavlink_msg_landing_target.h │ │ │ ├── mavlink_msg_link_node_status.h │ │ │ ├── mavlink_msg_local_position_ned.h │ │ │ ├── mavlink_msg_local_position_ned_cov.h │ │ │ ├── mavlink_msg_local_position_ned_system_global_offset.h │ │ │ ├── mavlink_msg_log_data.h │ │ │ ├── mavlink_msg_log_entry.h │ │ │ ├── mavlink_msg_log_erase.h │ │ │ ├── mavlink_msg_log_request_data.h │ │ │ ├── mavlink_msg_log_request_end.h │ │ │ ├── mavlink_msg_log_request_list.h │ │ │ ├── mavlink_msg_logging_ack.h │ │ │ ├── mavlink_msg_logging_data.h │ │ │ ├── mavlink_msg_logging_data_acked.h │ │ │ ├── mavlink_msg_mag_cal_report.h │ │ │ ├── mavlink_msg_manual_control.h │ │ │ ├── mavlink_msg_manual_setpoint.h │ │ │ ├── mavlink_msg_memory_vect.h │ │ │ ├── mavlink_msg_message_interval.h │ │ │ ├── mavlink_msg_mission_ack.h │ │ │ ├── mavlink_msg_mission_clear_all.h │ │ │ ├── mavlink_msg_mission_count.h │ │ │ ├── mavlink_msg_mission_current.h │ │ │ ├── mavlink_msg_mission_item.h │ │ │ ├── mavlink_msg_mission_item_int.h │ │ │ ├── mavlink_msg_mission_item_reached.h │ │ │ ├── mavlink_msg_mission_request.h │ │ │ ├── mavlink_msg_mission_request_int.h │ │ │ ├── mavlink_msg_mission_request_list.h │ │ │ ├── mavlink_msg_mission_request_partial_list.h │ │ │ ├── mavlink_msg_mission_set_current.h │ │ │ ├── mavlink_msg_mission_write_partial_list.h │ │ │ ├── mavlink_msg_mount_orientation.h │ │ │ ├── mavlink_msg_named_value_float.h │ │ │ ├── mavlink_msg_named_value_int.h │ │ │ ├── mavlink_msg_nav_controller_output.h │ │ │ ├── mavlink_msg_obstacle_distance.h │ │ │ ├── mavlink_msg_odometry.h │ │ │ ├── mavlink_msg_onboard_computer_status.h │ │ │ ├── mavlink_msg_open_drone_id_arm_status.h │ │ │ ├── mavlink_msg_open_drone_id_authentication.h │ │ │ ├── mavlink_msg_open_drone_id_basic_id.h │ │ │ ├── mavlink_msg_open_drone_id_location.h │ │ │ ├── mavlink_msg_open_drone_id_message_pack.h │ │ │ ├── mavlink_msg_open_drone_id_operator_id.h │ │ │ ├── mavlink_msg_open_drone_id_self_id.h │ │ │ ├── mavlink_msg_open_drone_id_system.h │ │ │ ├── mavlink_msg_open_drone_id_system_update.h │ │ │ ├── mavlink_msg_optical_flow.h │ │ │ ├── mavlink_msg_optical_flow_rad.h │ │ │ ├── mavlink_msg_orbit_execution_status.h │ │ │ ├── mavlink_msg_param_ext_ack.h │ │ │ ├── mavlink_msg_param_ext_request_list.h │ │ │ ├── mavlink_msg_param_ext_request_read.h │ │ │ ├── mavlink_msg_param_ext_set.h │ │ │ ├── mavlink_msg_param_ext_value.h │ │ │ ├── mavlink_msg_param_map_rc.h │ │ │ ├── mavlink_msg_param_request_list.h │ │ │ ├── mavlink_msg_param_request_read.h │ │ │ ├── mavlink_msg_param_set.h │ │ │ ├── mavlink_msg_param_value.h │ │ │ ├── mavlink_msg_ping.h │ │ │ ├── mavlink_msg_play_tune.h │ │ │ ├── mavlink_msg_play_tune_v2.h │ │ │ ├── mavlink_msg_position_target_global_int.h │ │ │ ├── mavlink_msg_position_target_local_ned.h │ │ │ ├── mavlink_msg_power_status.h │ │ │ ├── mavlink_msg_radio_status.h │ │ │ ├── mavlink_msg_raw_imu.h │ │ │ ├── mavlink_msg_raw_pressure.h │ │ │ ├── mavlink_msg_raw_rpm.h │ │ │ ├── mavlink_msg_rc_channels.h │ │ │ ├── mavlink_msg_rc_channels_override.h │ │ │ ├── mavlink_msg_rc_channels_raw.h │ │ │ ├── mavlink_msg_rc_channels_scaled.h │ │ │ ├── mavlink_msg_request_data_stream.h │ │ │ ├── mavlink_msg_request_event.h │ │ │ ├── mavlink_msg_resource_request.h │ │ │ ├── mavlink_msg_response_event_error.h │ │ │ ├── mavlink_msg_safety_allowed_area.h │ │ │ ├── mavlink_msg_safety_set_allowed_area.h │ │ │ ├── mavlink_msg_scaled_imu.h │ │ │ ├── mavlink_msg_scaled_imu2.h │ │ │ ├── mavlink_msg_scaled_imu3.h │ │ │ ├── mavlink_msg_scaled_pressure.h │ │ │ ├── mavlink_msg_scaled_pressure2.h │ │ │ ├── mavlink_msg_scaled_pressure3.h │ │ │ ├── mavlink_msg_serial_control.h │ │ │ ├── mavlink_msg_servo_output_raw.h │ │ │ ├── mavlink_msg_set_actuator_control_target.h │ │ │ ├── mavlink_msg_set_attitude_target.h │ │ │ ├── mavlink_msg_set_gps_global_origin.h │ │ │ ├── mavlink_msg_set_home_position.h │ │ │ ├── mavlink_msg_set_mode.h │ │ │ ├── mavlink_msg_set_position_target_global_int.h │ │ │ ├── mavlink_msg_set_position_target_local_ned.h │ │ │ ├── mavlink_msg_setup_signing.h │ │ │ ├── mavlink_msg_sim_state.h │ │ │ ├── mavlink_msg_statustext.h │ │ │ ├── mavlink_msg_storage_information.h │ │ │ ├── mavlink_msg_supported_tunes.h │ │ │ ├── mavlink_msg_sys_status.h │ │ │ ├── mavlink_msg_system_time.h │ │ │ ├── mavlink_msg_terrain_check.h │ │ │ ├── mavlink_msg_terrain_data.h │ │ │ ├── mavlink_msg_terrain_report.h │ │ │ ├── mavlink_msg_terrain_request.h │ │ │ ├── mavlink_msg_time_estimate_to_target.h │ │ │ ├── mavlink_msg_timesync.h │ │ │ ├── mavlink_msg_trajectory_representation_bezier.h │ │ │ ├── mavlink_msg_trajectory_representation_waypoints.h │ │ │ ├── mavlink_msg_tunnel.h │ │ │ ├── mavlink_msg_uavcan_node_info.h │ │ │ ├── mavlink_msg_uavcan_node_status.h │ │ │ ├── mavlink_msg_utm_global_position.h │ │ │ ├── mavlink_msg_v2_extension.h │ │ │ ├── mavlink_msg_vfr_hud.h │ │ │ ├── mavlink_msg_vibration.h │ │ │ ├── mavlink_msg_vicon_position_estimate.h │ │ │ ├── mavlink_msg_video_stream_information.h │ │ │ ├── mavlink_msg_video_stream_status.h │ │ │ ├── mavlink_msg_vision_position_estimate.h │ │ │ ├── mavlink_msg_vision_speed_estimate.h │ │ │ ├── mavlink_msg_wheel_distance.h │ │ │ ├── mavlink_msg_wifi_config_ap.h │ │ │ ├── mavlink_msg_winch_status.h │ │ │ ├── mavlink_msg_wind_cov.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── csAirLink │ │ │ ├── csAirLink.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_airlink_auth.h │ │ │ ├── mavlink_msg_airlink_auth_response.h │ │ │ ├── mavlink_msg_airlink_eye_gs_hole_push_request.h │ │ │ ├── mavlink_msg_airlink_eye_gs_hole_push_response.h │ │ │ ├── mavlink_msg_airlink_eye_hp.h │ │ │ ├── mavlink_msg_airlink_eye_turn_init.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── cubepilot │ │ │ ├── cubepilot.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_cubepilot_firmware_update_resp.h │ │ │ ├── mavlink_msg_cubepilot_firmware_update_start.h │ │ │ ├── mavlink_msg_cubepilot_raw_rc.h │ │ │ ├── mavlink_msg_herelink_telem.h │ │ │ ├── mavlink_msg_herelink_video_stream_information.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── development │ │ │ ├── development.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_airspeed.h │ │ │ ├── mavlink_msg_available_modes.h │ │ │ ├── mavlink_msg_available_modes_monitor.h │ │ │ ├── mavlink_msg_battery_status_v2.h │ │ │ ├── mavlink_msg_current_mode.h │ │ │ ├── mavlink_msg_figure_eight_execution_status.h │ │ │ ├── mavlink_msg_fuel_status.h │ │ │ ├── mavlink_msg_gnss_integrity.h │ │ │ ├── mavlink_msg_group_end.h │ │ │ ├── mavlink_msg_group_start.h │ │ │ ├── mavlink_msg_param_ack_transaction.h │ │ │ ├── mavlink_msg_radio_rc_channels.h │ │ │ ├── mavlink_msg_set_velocity_limits.h │ │ │ ├── mavlink_msg_target_absolute.h │ │ │ ├── mavlink_msg_target_relative.h │ │ │ ├── mavlink_msg_velocity_limits.h │ │ │ ├── mavlink_msg_wifi_network_info.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── icarous │ │ │ ├── icarous.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_icarous_heartbeat.h │ │ │ ├── mavlink_msg_icarous_kinematic_bands.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── matrixpilot │ │ │ ├── matrixpilot.h │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_airspeeds.h │ │ │ ├── mavlink_msg_altitudes.h │ │ │ ├── mavlink_msg_flexifunction_buffer_function.h │ │ │ ├── mavlink_msg_flexifunction_buffer_function_ack.h │ │ │ ├── mavlink_msg_flexifunction_command.h │ │ │ ├── mavlink_msg_flexifunction_command_ack.h │ │ │ ├── mavlink_msg_flexifunction_directory.h │ │ │ ├── mavlink_msg_flexifunction_directory_ack.h │ │ │ ├── mavlink_msg_flexifunction_read_req.h │ │ │ ├── mavlink_msg_flexifunction_set.h │ │ │ ├── mavlink_msg_serial_udb_extra_f13.h │ │ │ ├── mavlink_msg_serial_udb_extra_f14.h │ │ │ ├── mavlink_msg_serial_udb_extra_f15.h │ │ │ ├── mavlink_msg_serial_udb_extra_f16.h │ │ │ ├── mavlink_msg_serial_udb_extra_f17.h │ │ │ ├── mavlink_msg_serial_udb_extra_f18.h │ │ │ ├── mavlink_msg_serial_udb_extra_f19.h │ │ │ ├── mavlink_msg_serial_udb_extra_f20.h │ │ │ ├── mavlink_msg_serial_udb_extra_f21.h │ │ │ ├── mavlink_msg_serial_udb_extra_f22.h │ │ │ ├── mavlink_msg_serial_udb_extra_f2_a.h │ │ │ ├── mavlink_msg_serial_udb_extra_f2_b.h │ │ │ ├── mavlink_msg_serial_udb_extra_f4.h │ │ │ ├── mavlink_msg_serial_udb_extra_f5.h │ │ │ ├── mavlink_msg_serial_udb_extra_f6.h │ │ │ ├── mavlink_msg_serial_udb_extra_f7.h │ │ │ ├── mavlink_msg_serial_udb_extra_f8.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── mavlink_conversions.h │ │ ├── mavlink_get_info.h │ │ ├── mavlink_helpers.h │ │ ├── mavlink_sha256.h │ │ ├── mavlink_types.h │ │ ├── message_definitions │ │ │ ├── ASLUAV.xml │ │ │ ├── AVSSUAS.xml │ │ │ ├── all.xml │ │ │ ├── ardupilotmega.xml │ │ │ ├── common.xml │ │ │ ├── csAirLink.xml │ │ │ ├── cubepilot.xml │ │ │ ├── development.xml │ │ │ ├── icarous.xml │ │ │ ├── matrixpilot.xml │ │ │ ├── minimal.xml │ │ │ ├── paparazzi.xml │ │ │ ├── python_array_test.xml │ │ │ ├── standard.xml │ │ │ ├── storm32.xml │ │ │ ├── test.xml │ │ │ ├── uAvionix.xml │ │ │ └── ualberta.xml │ │ ├── minimal │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_heartbeat.h │ │ │ ├── mavlink_msg_protocol_version.h │ │ │ ├── minimal.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── paparazzi │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_script_count.h │ │ │ ├── mavlink_msg_script_current.h │ │ │ ├── mavlink_msg_script_item.h │ │ │ ├── mavlink_msg_script_request.h │ │ │ ├── mavlink_msg_script_request_list.h │ │ │ ├── paparazzi.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── protocol.h │ │ ├── python_array_test │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_array_test_0.h │ │ │ ├── mavlink_msg_array_test_1.h │ │ │ ├── mavlink_msg_array_test_3.h │ │ │ ├── mavlink_msg_array_test_4.h │ │ │ ├── mavlink_msg_array_test_5.h │ │ │ ├── mavlink_msg_array_test_6.h │ │ │ ├── mavlink_msg_array_test_7.h │ │ │ ├── mavlink_msg_array_test_8.h │ │ │ ├── python_array_test.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── standard │ │ │ ├── mavlink.h │ │ │ ├── standard.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── storm32 │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_frsky_passthrough_array.h │ │ │ ├── mavlink_msg_param_value_array.h │ │ │ ├── mavlink_msg_qshot_status.h │ │ │ ├── mavlink_msg_storm32_gimbal_manager_control.h │ │ │ ├── mavlink_msg_storm32_gimbal_manager_control_pitchyaw.h │ │ │ ├── mavlink_msg_storm32_gimbal_manager_correct_roll.h │ │ │ ├── mavlink_msg_storm32_gimbal_manager_information.h │ │ │ ├── mavlink_msg_storm32_gimbal_manager_status.h │ │ │ ├── storm32.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── test │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_test_types.h │ │ │ ├── test.h │ │ │ ├── testsuite.h │ │ │ └── version.h │ │ ├── uAvionix │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_uavionix_adsb_out_cfg.h │ │ │ ├── mavlink_msg_uavionix_adsb_out_dynamic.h │ │ │ ├── mavlink_msg_uavionix_adsb_transceiver_health_report.h │ │ │ ├── testsuite.h │ │ │ ├── uAvionix.h │ │ │ └── version.h │ │ └── ualberta │ │ │ ├── mavlink.h │ │ │ ├── mavlink_msg_nav_filter_bias.h │ │ │ ├── mavlink_msg_radio_calibration.h │ │ │ ├── mavlink_msg_ualberta_sys_status.h │ │ │ ├── testsuite.h │ │ │ ├── ualberta.h │ │ │ └── version.h │ ├── libmodbus │ │ ├── config.h │ │ ├── modbus-data.c │ │ ├── modbus-private.h │ │ ├── modbus-rtu-private.h │ │ ├── modbus-rtu.c │ │ ├── modbus-rtu.h │ │ ├── modbus-tcp-private.h │ │ ├── modbus-tcp.c │ │ ├── modbus-tcp.h │ │ ├── modbus-version.h │ │ ├── modbus.c │ │ └── modbus.h │ ├── minmea.c │ ├── minmea.h │ └── picojson.h ├── Detector │ ├── _ArUco.cpp │ ├── _ArUco.h │ ├── _Cascade.cpp │ ├── _Cascade.h │ ├── _Chilitags.cpp │ ├── _Chilitags.h │ ├── _Contour.cpp │ ├── _Contour.h │ ├── _DNNclassifier.cpp │ ├── _DNNclassifier.h │ ├── _DNNtext.cpp │ ├── _DNNtext.h │ ├── _DepthSegment.cpp │ ├── _DepthSegment.h │ ├── _DetectorBase.cpp │ ├── _DetectorBase.h │ ├── _HandKey.cpp │ ├── _HandKey.h │ ├── _IRLock.cpp │ ├── _IRLock.h │ ├── _Lane.cpp │ ├── _Lane.h │ ├── _Line.cpp │ ├── _Line.h │ ├── _MotionDetector.cpp │ ├── _MotionDetector.h │ ├── _OpenPose.cpp │ ├── _OpenPose.h │ ├── _SSD.cpp │ ├── _SSD.h │ ├── _YOLOv3.cpp │ ├── _YOLOv3.h │ ├── _YOLOv8.cpp │ └── _YOLOv8.h ├── Filter │ ├── Average.h │ ├── FilterBase.h │ ├── Median.h │ └── Predict.h ├── IO │ ├── _ADIO_EBYTE.cpp │ ├── _ADIO_EBYTE.h │ ├── _ADIObase.cpp │ ├── _ADIObase.h │ ├── _File.cpp │ ├── _File.h │ ├── _IObase.cpp │ ├── _IObase.h │ ├── _SerialPort.cpp │ ├── _SerialPort.h │ ├── _TCPclient.cpp │ ├── _TCPclient.h │ ├── _TCPserver.cpp │ ├── _TCPserver.h │ ├── _UDP.cpp │ ├── _UDP.h │ ├── _WebSocket.cpp │ ├── _WebSocket.h │ ├── _WebSocketServer.cpp │ └── _WebSocketServer.h ├── IPC │ ├── SharedMem.cpp │ └── SharedMem.h ├── Module │ ├── JSON.cpp │ ├── JSON.h │ ├── Kiss.cpp │ ├── Kiss.h │ ├── Module.cpp │ └── Module.h ├── Navigation │ ├── Coordinate.cpp │ ├── Coordinate.h │ ├── GeoGrid.cpp │ ├── GeoGrid.h │ ├── _GPS.cpp │ ├── _GPS.h │ ├── _NavBase.cpp │ ├── _NavBase.h │ ├── _RStracking.cpp │ ├── _RStracking.h │ ├── _RTCM3.cpp │ └── _RTCM3.h ├── Net │ ├── HttpClient.cpp │ ├── HttpClient.h │ ├── _Uploader.cpp │ ├── _Uploader.h │ ├── base64.cpp │ └── base64.h ├── OpenKAI.cpp ├── OpenKAI.h ├── Primitive │ ├── primitiveUtil.h │ ├── tSwap.h │ ├── vDouble2.h │ ├── vDouble3.h │ ├── vDouble4.h │ ├── vFloat2.h │ ├── vFloat3.h │ ├── vFloat4.h │ ├── vInt2.h │ ├── vInt3.h │ └── vInt4.h ├── Protocol │ ├── _JSONbase.cpp │ ├── _JSONbase.h │ ├── _Mavlink.cpp │ ├── _Mavlink.h │ ├── _Modbus.cpp │ ├── _Modbus.h │ ├── _PWMio.cpp │ ├── _PWMio.h │ ├── _ProtocolBase.cpp │ ├── _ProtocolBase.h │ ├── _SBus.cpp │ ├── _SBus.h │ ├── _USR_CANET.cpp │ ├── _USR_CANET.h │ ├── _Xbee.cpp │ └── _Xbee.h ├── ROS │ ├── ROS_fastLio.cpp │ ├── ROS_fastLio.h │ ├── _ROS_fastLio.cpp │ └── _ROS_fastLio.h ├── RobotArm │ ├── _Sequencer.cpp │ └── _Sequencer.h ├── SLAM │ ├── _SLAMbase.cpp │ └── _SLAMbase.h ├── Science │ ├── _IsingSolver.cpp │ └── _IsingSolver.h ├── Sensor │ ├── Distance │ │ ├── _BenewakeTF.cpp │ │ ├── _BenewakeTF.h │ │ ├── _DistSensorBase.cpp │ │ ├── _DistSensorBase.h │ │ ├── _LeddarVu.cpp │ │ ├── _LeddarVu.h │ │ ├── _TOFsense.cpp │ │ └── _TOFsense.h │ └── LiDAR │ │ ├── _Livox2.cpp │ │ ├── _Livox2.h │ │ ├── _RoboSenseAiry.cpp │ │ ├── _RoboSenseAiry.h │ │ └── livox_lidar_def.h ├── State │ ├── StateBase.cpp │ ├── StateBase.h │ ├── _StateControl.cpp │ └── _StateControl.h ├── Swarm │ ├── _SwarmBase.cpp │ ├── _SwarmBase.h │ ├── _SwarmCtrl.cpp │ ├── _SwarmCtrl.h │ ├── _SwarmCtrlUI.cpp │ ├── _SwarmCtrlUI.h │ ├── _SwarmSearch.cpp │ └── _SwarmSearch.h ├── Tracker │ ├── _SingleTracker.cpp │ ├── _SingleTracker.h │ ├── _TrackerBase.cpp │ └── _TrackerBase.h ├── UI │ ├── O3DUI.cpp │ ├── O3DUI.h │ ├── _Console.cpp │ ├── _Console.h │ ├── _GstOutput.cpp │ ├── _GstOutput.h │ ├── _UIbase.cpp │ ├── _UIbase.h │ ├── _WindowCV.cpp │ └── _WindowCV.h ├── Universe │ ├── _Object.cpp │ ├── _Object.h │ ├── _ObjectArray.cpp │ ├── _ObjectArray.h │ ├── _Universe.cpp │ └── _Universe.h ├── Utility │ ├── BitFlag.h │ ├── RC.h │ ├── util.h │ ├── utilCV.h │ ├── utilEvent.h │ ├── utilFile.h │ ├── utilNet.h │ ├── utilStr.h │ ├── utilTime.h │ └── utilVar.h ├── Vision │ ├── Frame.h │ ├── FrameBase.cpp │ ├── FrameBase.h │ ├── FrameGPU.cpp │ ├── FrameGPU.h │ ├── ImgFilter │ │ ├── _ColorConvert.cpp │ │ ├── _ColorConvert.h │ │ ├── _Contrast.cpp │ │ ├── _Contrast.h │ │ ├── _Crop.cpp │ │ ├── _Crop.h │ │ ├── _Depth2Gray.cpp │ │ ├── _Depth2Gray.h │ │ ├── _DepthProj.cpp │ │ ├── _DepthProj.h │ │ ├── _DepthShow.cpp │ │ ├── _DepthShow.h │ │ ├── _Erode.cpp │ │ ├── _Erode.h │ │ ├── _HistEqualize.cpp │ │ ├── _HistEqualize.h │ │ ├── _InRange.cpp │ │ ├── _InRange.h │ │ ├── _Invert.cpp │ │ ├── _Invert.h │ │ ├── _Mask.cpp │ │ ├── _Mask.h │ │ ├── _Morphology.cpp │ │ ├── _Morphology.h │ │ ├── _Remap.cpp │ │ ├── _Remap.h │ │ ├── _Resize.cpp │ │ ├── _Resize.h │ │ ├── _Rotate.cpp │ │ ├── _Rotate.h │ │ ├── _Threshold.cpp │ │ └── _Threshold.h │ ├── RGBD │ │ ├── _Orbbec.cpp │ │ ├── _Orbbec.h │ │ ├── _RGBDbase.cpp │ │ ├── _RGBDbase.h │ │ ├── _RealSense.cpp │ │ ├── _RealSense.h │ │ ├── _Vzense.cpp │ │ ├── _Vzense.h │ │ ├── _XDynamics.cpp │ │ └── _XDynamics.h │ ├── _CamCalib.cpp │ ├── _CamCalib.h │ ├── _Camera.cpp │ ├── _Camera.h │ ├── _DenseFlow.cpp │ ├── _DenseFlow.h │ ├── _GPhoto.cpp │ ├── _GPhoto.h │ ├── _GStreamer.cpp │ ├── _GStreamer.h │ ├── _ImgFile.cpp │ ├── _ImgFile.h │ ├── _InfiRay.cpp │ ├── _InfiRay.h │ ├── _SharedMemImg.cpp │ ├── _SharedMemImg.h │ ├── _UVC.cpp │ ├── _UVC.h │ ├── _VideoFile.cpp │ ├── _VideoFile.h │ ├── _VisionBase.cpp │ └── _VisionBase.h └── main.cpp └── test ├── IO ├── _TestWebSocket.cpp └── _TestWebSocket.h ├── Protocol ├── _TestJSON.cpp └── _TestJSON.h ├── _TestBase.cpp └── _TestBase.h /.gitignore: -------------------------------------------------------------------------------- 1 | /data/output/*.*log 2 | /Debug/ 3 | /Release/ 4 | /build/ 5 | *.prefs 6 | .project 7 | .cproject 8 | *.*~ 9 | /.kdev4/ 10 | null.d 11 | !.gitignore 12 | *.log 13 | 14 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**" 7 | ], 8 | "defines": [], 9 | "compilerPath": "/usr/bin/gcc", 10 | "cStandard": "c11", 11 | "cppStandard": "c++14", 12 | "intelliSenseMode": "linux-gcc-x64", 13 | "compileCommands": "${workspaceFolder}/build/compile_commands.json", 14 | "configurationProvider": "ms-vscode.cmake-tools" 15 | } 16 | ], 17 | "version": 4 18 | } -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "(gdb) Launch", 9 | "type": "cppdbg", 10 | "request": "launch", 11 | "program": "${workspaceFolder}/build/OpenKAI", 12 | "args": ["${workspaceFolder}/kiss/_PCgrid.kiss"], 13 | "stopAtEntry": false, 14 | "cwd": "${workspaceFolder}", 15 | "environment": [], 16 | "MIMode": "gdb", 17 | "setupCommands": [ 18 | { 19 | "description": "Enable pretty-printing for gdb", 20 | "text": "-enable-pretty-printing", 21 | "ignoreFailures": true 22 | } 23 | ] 24 | } 25 | ] 26 | } -------------------------------------------------------------------------------- /data/AprilTags/Tag16h5.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/AprilTags/Tag16h5.pdf -------------------------------------------------------------------------------- /data/AprilTags/Tag25h9.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/AprilTags/Tag25h9.pdf -------------------------------------------------------------------------------- /data/AprilTags/Tag36h11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/AprilTags/Tag36h11.pdf -------------------------------------------------------------------------------- /data/LandingPad/ArucoPad_Tag16h5.ai: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/LandingPad/ArucoPad_Tag16h5.ai -------------------------------------------------------------------------------- /data/LandingPad/LandingPad_16h5x6p.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/LandingPad/LandingPad_16h5x6p.pdf -------------------------------------------------------------------------------- /data/Livox/HAP_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk": true, 3 | "lidar_log_enable": false, 4 | "lidar_log_cache_size_MB": 1, 5 | "lidar_log_path": "./", 6 | "HAP": { 7 | "lidar_net_info": { 8 | "cmd_data_port": 56000, 9 | "push_msg_port": 0, 10 | "point_data_port": 57000, 11 | "imu_data_port": 58000, 12 | "log_data_port": 59000 13 | }, 14 | "host_net_info": { 15 | "cmd_data_ip": "192.168.1.5", 16 | "cmd_data_port": 56000, 17 | "push_msg_ip": "", 18 | "push_msg_port": 0, 19 | "point_data_ip": "192.168.1.5", 20 | "point_data_port": 57000, 21 | "imu_data_ip": "192.168.1.5", 22 | "imu_data_port": 58000, 23 | "log_data_ip": "", 24 | "log_data_port": 59000 25 | } 26 | }, 27 | "lidar_configs": [ 28 | { 29 | "ip": "192.168.1.100", 30 | "pcl_data_type": 1, 31 | "pattern_mode": 0, 32 | "extrinsic_parameter": { 33 | "roll": 0.0, 34 | "pitch": 0.0, 35 | "yaw": 0.0, 36 | "x": 0, 37 | "y": 0, 38 | "z": 0 39 | } 40 | } 41 | ] 42 | } -------------------------------------------------------------------------------- /data/Livox/mid360_200.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.200.117"], 21 | "host_ip" : "192.168.200.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | ] 30 | } 31 | } -------------------------------------------------------------------------------- /data/Livox/mid360_201.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.201.139"], 21 | "host_ip" : "192.168.201.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | ] 30 | } 31 | } -------------------------------------------------------------------------------- /data/Livox/mid360_202.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.202.178"], 21 | "host_ip" : "192.168.202.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | ] 30 | } 31 | } -------------------------------------------------------------------------------- /data/Livox/mid360_203.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.203.167"], 21 | "host_ip" : "192.168.203.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | ] 30 | } 31 | } -------------------------------------------------------------------------------- /data/Livox/mid360_204.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.204.198"], 21 | "host_ip" : "192.168.204.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | ] 30 | } 31 | } -------------------------------------------------------------------------------- /data/Livox/mid360_8.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.8.101"], 21 | "host_ip" : "192.168.8.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | ] 30 | } 31 | } -------------------------------------------------------------------------------- /data/Livox/mid360_8_103.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.8.103"], 21 | "host_ip" : "192.168.8.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | ] 30 | } 31 | } -------------------------------------------------------------------------------- /data/Livox/mid360_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "master_sdk" : true, 3 | "lidar_log_enable" : false, 4 | "lidar_log_cache_size_MB" : 1, 5 | "lidar_log_path" : "./", 6 | 7 | "MID360": 8 | { 9 | "lidar_net_info" : 10 | { 11 | "cmd_data_port" : 56100, 12 | "push_msg_port" : 56200, 13 | "point_data_port": 56300, 14 | "imu_data_port" : 56400, 15 | "log_data_port" : 56500 16 | }, 17 | "host_net_info" : 18 | [ 19 | { 20 | "lidar_ip" : ["192.168.177.177"], 21 | "host_ip" : "192.168.177.10", 22 | "multicast_ip" : "224.1.1.5", 23 | "cmd_data_port" : 56101, 24 | "push_msg_port" : 56201, 25 | "point_data_port": 56301, 26 | "imu_data_port" : 56401, 27 | "log_data_port" : 56501 28 | } 29 | , 30 | { 31 | "lidar_ip" : ["192.168.173.173"], 32 | "host_ip" : "192.168.173.10", 33 | "multicast_ip" : "224.1.1.5", 34 | "cmd_data_port" : 56101, 35 | "push_msg_port" : 56201, 36 | "point_data_port": 56301, 37 | "imu_data_port" : 56401, 38 | "log_data_port" : 56501 39 | } 40 | 41 | ] 42 | } 43 | } -------------------------------------------------------------------------------- /data/OK.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/OK.png -------------------------------------------------------------------------------- /data/calibration/circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/calibration/circle.png -------------------------------------------------------------------------------- /data/calibration/pattern.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/data/calibration/pattern.png -------------------------------------------------------------------------------- /firmware/arduino/arduLED.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | struct PIN_IO 4 | { 5 | int m_in; 6 | int m_out; 7 | 8 | void init(int in, int out) 9 | { 10 | m_in = in; 11 | m_out = out; 12 | 13 | pinMode(m_in, INPUT); 14 | pinMode(m_out, OUTPUT); 15 | } 16 | 17 | void update(void) 18 | { 19 | digitalWrite(m_out, digitalRead(m_in)); 20 | } 21 | }; 22 | 23 | #define N_IO 2 24 | PIN_IO g_pinIO[N_IO]; 25 | 26 | void setup() 27 | { 28 | g_pinIO[0].init(14,9); 29 | g_pinIO[1].init(15,7); 30 | } 31 | 32 | void loop() 33 | { 34 | for(int i=0; i 4 | #include "BLC.h" 5 | 6 | #define PWM_MID 1500 7 | #define PWM_D 500 8 | #define PWM_MID_DZ 25 9 | #define PWM_MID_DZ_BTN 100 10 | #define PWM_LOWLIM 600 11 | 12 | struct MOTOR 13 | { 14 | Servo m_servo; 15 | int16_t m_pwm; 16 | int16_t m_pwmL; 17 | int16_t m_pwmM; 18 | int16_t m_pwmH; 19 | int8_t m_pin; 20 | int8_t m_dir; 21 | 22 | void init(int16_t pwmM, int16_t pwmL, int16_t pwmH, int8_t pin, int8_t dir) 23 | { 24 | m_pwmL = pwmL; 25 | m_pwmM = pwmM; 26 | m_pwmH = pwmH; 27 | m_pwm = m_pwmM; 28 | m_pin = pin; 29 | m_dir = dir; 30 | 31 | pinMode(m_pin, OUTPUT); 32 | m_servo.attach(m_pin); 33 | m_servo.writeMicroseconds(m_pwm); 34 | } 35 | 36 | void speed(float s) 37 | { 38 | float d = constrain(s, -1.0, 1.0); 39 | 40 | if (d >= 0.0) 41 | m_pwm = PWM_MID + d * ((float)(m_pwmH - m_pwmM) * m_dir); 42 | else 43 | m_pwm = PWM_MID + d * ((float)(m_pwmM - m_pwmL) * m_dir); 44 | 45 | m_pwm = constrain(m_pwm, 1400, 1600); 46 | 47 | m_servo.writeMicroseconds(m_pwm); 48 | } 49 | }; 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /firmware/arduino/include/arduMotorPWM.h: -------------------------------------------------------------------------------- 1 | #ifndef MOT_PWM_h 2 | #define MOT_PWM_h 3 | #include 4 | 5 | #define PWM_MID 1500 6 | #define PWM_D 500 7 | #define PWM_MID_DZ 25 8 | #define PWM_MID_DZ_BTN 100 9 | #define PWM_LOWLIM 600 10 | 11 | struct MOTOR 12 | { 13 | Servo m_servo; 14 | int16_t m_pwm; 15 | int16_t m_pwmL; 16 | int16_t m_pwmM; 17 | int16_t m_pwmH; 18 | int8_t m_pin; 19 | int8_t m_dir; 20 | 21 | void init(int16_t pwmM, int16_t pwmL, int16_t pwmH, int8_t pin, int8_t dir) 22 | { 23 | m_pwmL = pwmL; 24 | m_pwmM = pwmM; 25 | m_pwmH = pwmH; 26 | m_pwm = m_pwmM; 27 | m_pin = pin; 28 | m_dir = dir; 29 | 30 | pinMode(m_pin, OUTPUT); 31 | m_servo.attach(m_pin); 32 | m_servo.writeMicroseconds(m_pwm); 33 | } 34 | 35 | void speed(float s) 36 | { 37 | float d = constrain(s, -1.0, 1.0); 38 | 39 | if (d >= 0.0) 40 | m_pwm = PWM_MID + d * ((float)(m_pwmH - m_pwmM) * m_dir); 41 | else 42 | m_pwm = PWM_MID + d * ((float)(m_pwmM - m_pwmL) * m_dir); 43 | 44 | m_pwm = constrain(m_pwm, 1400, 1600); 45 | 46 | m_servo.writeMicroseconds(m_pwm); 47 | } 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /firmware/arduino/include/arduOK.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | //0 PROTOCOL_BEGIN 4 | //1 COMMAND 5 | //2 PAYLOAD LENGTH 6 | //3 Payload... 7 | 8 | //Protocol 9 | #define PB_BEGIN 0xFE 10 | #define PB_N_HDR 3 11 | #define PROTOCOL_N_BUF 256 12 | 13 | //Functions to be implemented 14 | void runCMD(void); 15 | bool ioAvailable(void); 16 | byte ioRead(void); 17 | 18 | int g_nMsg = 1; 19 | 20 | struct OKLINK_CMD 21 | { 22 | uint8_t m_cmd; 23 | uint8_t m_nPayload; 24 | uint16_t m_iByte; 25 | uint8_t m_pBuf[PROTOCOL_N_BUF]; 26 | }; 27 | OKLINK_CMD g_cmd; 28 | 29 | void decodeCMD(void) 30 | { 31 | byte inByte; 32 | int iMsg = 0; 33 | 34 | while (ioAvailable() > 0) 35 | { 36 | inByte = ioRead(); 37 | 38 | if (g_cmd.m_cmd != 0) 39 | { 40 | g_cmd.m_pBuf[g_cmd.m_iByte] = inByte; 41 | g_cmd.m_iByte++; 42 | 43 | if (g_cmd.m_iByte == 3) //Payload length 44 | { 45 | g_cmd.m_nPayload = inByte; 46 | } 47 | else if (g_cmd.m_iByte == g_cmd.m_nPayload + PB_N_HDR ) 48 | { 49 | runCMD(); 50 | g_cmd.m_cmd = 0; 51 | 52 | if (++iMsg > g_nMsg)return; 53 | } 54 | } 55 | else if (inByte == PB_BEGIN ) 56 | { 57 | g_cmd.m_cmd = inByte; 58 | g_cmd.m_pBuf[0] = inByte; 59 | g_cmd.m_iByte = 1; 60 | g_cmd.m_nPayload = 0; 61 | } 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /kiss/OpenCL.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":10, 16 | } 17 | "vBASE":["cl"], 18 | } 19 | 20 | { 21 | "name":"cl", 22 | "class":"clBase", 23 | "bON":1, 24 | "bLog":1, 25 | "fKernel":"", 26 | "buildOpt":"", 27 | } 28 | -------------------------------------------------------------------------------- /kiss/_BenewakeTF03.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":1, 13 | { 14 | "name":"thread", 15 | "FPS":30, 16 | } 17 | "vBASE":["TF03"], 18 | } 19 | 20 | { 21 | "name":"serial", 22 | "class":"_SerialPort", 23 | "bON":1, 24 | { 25 | "name":"thread", 26 | "FPS":30, 27 | } 28 | { 29 | "name":"threadR", 30 | "FPS":30, 31 | } 32 | "bLog":0, 33 | "port":"/dev/ttyACM0", 34 | "/port":"/dev/ttyUSB0", 35 | "baud":115200, 36 | } 37 | 38 | { 39 | "name":"TF03", 40 | "class":"_BenewakeTF", 41 | "bON":1, 42 | { 43 | "name":"thread", 44 | "FPS":30, 45 | } 46 | "bLog":1, 47 | "_IObase":"serial", 48 | } 49 | -------------------------------------------------------------------------------- /kiss/_CamCalib.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bWindow":1, 6 | "bLog":1, 7 | "bStdErr":1, 8 | } 9 | 10 | { 11 | "name":"camCalib", 12 | "class":"_CamCalib", 13 | "bON":1, 14 | { 15 | "name":"thread", 16 | "FPS":10, 17 | } 18 | "path":"/home/kai/calib/\*.jpg", 19 | "vChessBoardSize":[9,6], 20 | "squareSize":0.03, 21 | } 22 | 23 | -------------------------------------------------------------------------------- /kiss/_Chilitags.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bWindow":1, 6 | "bDraw":1, 7 | "bLog":0, 8 | "bStdErr":1, 9 | "waitKey":30, 10 | } 11 | 12 | { 13 | "name":"OK_Console", 14 | "class":"_Console", 15 | "bON":1, 16 | } 17 | 18 | { 19 | "name":"OKview", 20 | "class":"Window", 21 | "bON":1, 22 | } 23 | 24 | { 25 | "name":"cam", 26 | "class":"_Camera", 27 | "FPS":30, 28 | "bON":1, 29 | "Window":"OKview", 30 | "_Console":"OK_Console", 31 | "deviceID":0, 32 | "w":640, 33 | "h":480, 34 | } 35 | 36 | { 37 | "name":"universe", 38 | "class":"_Universe", 39 | "bON":1, 40 | "Window":"OKview", 41 | "_Console":"OK_Console", 42 | } 43 | 44 | { 45 | "name":"Chilitags", 46 | "class":"_Chilitags", 47 | "FPS":30, 48 | "bON":1, 49 | "bDraw":1, 50 | "Window":"OKview", 51 | "_Console":"OK_Console", 52 | "_VisionBase":"cam", 53 | "_Universe":"universe", 54 | "persistence":0, 55 | "gain":0, 56 | } 57 | -------------------------------------------------------------------------------- /kiss/_Feetech.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":10, 16 | } 17 | "vBASE":["ft"], 18 | } 19 | 20 | { 21 | "name":"ft", 22 | "class":"_Feetech", 23 | "bON":1, 24 | "bLog":0, 25 | { 26 | "name":"thread", 27 | "FPS":10, 28 | } 29 | "port":"/dev/ttyUSB0", 30 | "/port":"/dev/ttyACM0", 31 | "baud":115200, 32 | "iID":1, 33 | 34 | { 35 | "name":"axis", 36 | { 37 | "name":"X", 38 | "pOrigin":0.0, 39 | "pErr":1, 40 | "pTarget":600, 41 | "pRange":[600,1600], 42 | "sTarget":10000.0, 43 | "sRange":[0,10000], 44 | "aTarget":100.0, 45 | "aRange":[1,300], 46 | } 47 | } 48 | } 49 | 50 | { 51 | "name":"modbus", 52 | "class":"_Modbus", 53 | "bON":0, 54 | "bLog":1, 55 | { 56 | "name":"thread", 57 | "FPS":30, 58 | } 59 | "port":"/dev/ttyUSB1", 60 | "baud":115200, 61 | "parity":"N", 62 | } 63 | 64 | -------------------------------------------------------------------------------- /kiss/_GDimgUploader.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bWindow":1, 6 | "bDraw":1, 7 | "bLog":1, 8 | "b_Console":0, 9 | "bStdErr":1, 10 | "waitKey":30, 11 | } 12 | 13 | { 14 | "name":"OKview", 15 | "class":"Window", 16 | "bON":1, 17 | } 18 | 19 | { 20 | "name":"cam", 21 | "class":"_Camera", 22 | "FPS":30, 23 | "bON":1, 24 | "Window":"OKview", 25 | "deviceID":0, 26 | "w":640, 27 | "h":480, 28 | } 29 | 30 | { 31 | "name":"cam", 32 | "class":"_GStreamer", 33 | "FPS":30, 34 | "bON":0, 35 | "Window":"OKview", 36 | "pipeline":"rtspsrc location=rtsp://admin:12345@10.0.0.100:554/live/main latency=0 ! decodebin ! videoconvert ! appsink", 37 | } 38 | 39 | { 40 | "name":"gdUpload", 41 | "class":"_GDimgUploader", 42 | "FPS":30, 43 | "bON":1, 44 | "bLog":1, 45 | "bDraw":1, 46 | "Window":"OKview", 47 | "tempDir":"/home/kai/dev/test/", 48 | "jpgQuality":80, 49 | "gdUpload":"python /home/kai/dev/workspace/OpenKAI/OpenKAI/python/GoogleAPI/gdUpload.py", 50 | "gdFolderID":"1Uz7Az_jhnpU8tUaIGOcek0qHA-T0gWBe", 51 | "gdCredentials":"/home/kai/credentials.json", 52 | } 53 | -------------------------------------------------------------------------------- /kiss/_GPS.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bWindow":0, 6 | "bLog":0, 7 | "b_Console":1, 8 | "bStdErr":0, 9 | "waitKey":30, 10 | } 11 | 12 | { 13 | "name":"serial", 14 | "class":"_SerialPort", 15 | "bON":1, 16 | "FPS":30, 17 | "bLog":0, 18 | "port":"/dev/ttyUSB0", 19 | "baud":115200, 20 | "_wakeUp":"GPS", 21 | } 22 | 23 | { 24 | "name":"GPS", 25 | "class":"_GPS", 26 | "FPS":30, 27 | "bON":1, 28 | "bLog":0, 29 | "_IObase":"serial", 30 | } 31 | 32 | -------------------------------------------------------------------------------- /kiss/_InfiRay.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":10, 16 | } 17 | "vBASE":["cam"], 18 | } 19 | 20 | { 21 | "name":"view", 22 | "class":"_WindowCV", 23 | "bON":1, 24 | { 25 | "name":"thread", 26 | "FPS":30, 27 | } 28 | "bFullScreen":0, 29 | "vBASE":["/iRay","/thr","/contrast","histEqualize"], 30 | } 31 | 32 | { 33 | "name":"iRay", 34 | "class":"_InfiRay", 35 | "bON":1, 36 | { 37 | "name":"thread", 38 | "FPS":30, 39 | } 40 | "deviceID":4, 41 | "vSize":[256,196], 42 | } 43 | 44 | { 45 | "name":"thr", 46 | "class":"_Threshold", 47 | "bON":0, 48 | { 49 | "name":"thread", 50 | "FPS":30, 51 | } 52 | "_VisionBase":"iRay", 53 | "type":1, 54 | } 55 | 56 | { 57 | "name":"histEqualize", 58 | "class":"_HistEqualize", 59 | "bON":1, 60 | { 61 | "name":"thread", 62 | "FPS":30, 63 | } 64 | "_VisionBase":"iRay", 65 | } 66 | 67 | { 68 | "name":"contrast", 69 | "class":"_Contrast", 70 | "bON":0, 71 | { 72 | "name":"thread", 73 | "FPS":30, 74 | } 75 | "_VisionBase":"iRay", 76 | "alpha":5.0, 77 | "beta":0.0, 78 | } 79 | -------------------------------------------------------------------------------- /kiss/_SharedMemImg.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":30, 16 | } 17 | "vBASE":["Vzense"], 18 | } 19 | 20 | { 21 | "name":"RGBview", 22 | "class":"_WindowCV", 23 | "bON":1, 24 | "bFullScreen":0, 25 | { 26 | "name":"thread", 27 | "FPS":30, 28 | } 29 | "vBASE":["SHMvzense"], 30 | } 31 | 32 | { 33 | "name":"depthView", 34 | "class":"_WindowCV", 35 | "bON":0, 36 | "bFullScreen":0, 37 | { 38 | "name":"thread", 39 | "FPS":30, 40 | } 41 | "vBASE":["DepthShow"], 42 | } 43 | 44 | { 45 | "name":"SHMvzRGB", 46 | "class":"SharedMem", 47 | "bON":1, 48 | { 49 | "name":"thread", 50 | "FPS":30, 51 | } 52 | "shmName":"SHMvzRGB", 53 | "nB":5760000, 54 | "bWriter":0, 55 | } 56 | 57 | { 58 | "name":"SHMvzense", 59 | "class":"SharedMemImg", 60 | { 61 | "name":"thread", 62 | "FPS":30, 63 | } 64 | "bON":1, 65 | "bLog":1, 66 | "vSize":[1600,1200], 67 | "SharedMem":"SHMvzRGB", 68 | } 69 | 70 | { 71 | "name":"DepthShow", 72 | "class":"_DepthShow", 73 | "bON":0, 74 | { 75 | "name":"thread", 76 | "FPS":30, 77 | } 78 | "_RGBDbase":"Vzense", 79 | } 80 | -------------------------------------------------------------------------------- /kiss/_TOFsense.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":0, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":1, 13 | { 14 | "name":"thread", 15 | "FPS":30, 16 | } 17 | "vBASE":["serial","TOFsense"], 18 | } 19 | 20 | { 21 | "name":"serial", 22 | "class":"_SerialPort", 23 | "bON":1, 24 | { 25 | "name":"thread", 26 | "FPS":30, 27 | } 28 | { 29 | "name":"threadR", 30 | "FPS":30, 31 | } 32 | "bLog":1, 33 | "port":"/dev/ttyAMA1", 34 | "baud":921600, 35 | } 36 | 37 | { 38 | "name":"TOFsense", 39 | "class":"_TOFsense", 40 | "bON":1, 41 | { 42 | "name":"thread", 43 | "FPS":30, 44 | } 45 | "bLog":1, 46 | "_IObase":"serial", 47 | } -------------------------------------------------------------------------------- /kiss/_Uploader.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":30, 16 | } 17 | "vBASE":["uploader"], 18 | } 19 | 20 | { 21 | "name":"uploader", 22 | "class":"_Uploader", 23 | "bON":1, 24 | { 25 | "name":"thread", 26 | "FPS":1, 27 | } 28 | "bLog":1, 29 | "dir":"/home/kai/Downloads/test/", 30 | "vExt":[".mka",".json",".png",".jpg"], 31 | "bRemoveAfterUpload":0, 32 | "method":0, 33 | "url":"http://183.6.39.6:9055/api/Uav/UavImageData", 34 | 35 | "/cmd":"uploader 2>&1 -T [fName] ftp://193.112.75.123/pub/ -u anonymous: --connect-timeout 60 --no-progress-meter", 36 | "bConfirmCmdResult":0, 37 | } 38 | -------------------------------------------------------------------------------- /kiss/_Vzense.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":1, 13 | { 14 | "name":"thread", 15 | "FPS":30, 16 | } 17 | "vBASE":["Vzense"], 18 | } 19 | 20 | { 21 | "name":"SHMvzRGB", 22 | "class":"SharedMem", 23 | "bON":0, 24 | { 25 | "name":"thread", 26 | "FPS":30, 27 | } 28 | "shmName":"SHMvzRGB", 29 | "nB":5760000, 30 | "bWriter":1, 31 | } 32 | 33 | { 34 | "name":"Vzense", 35 | "class":"_Vzense", 36 | { 37 | "name":"thread", 38 | "FPS":25, 39 | } 40 | { 41 | "name":"threadPP", 42 | "FPS":25, 43 | } 44 | "bON":1, 45 | "bLog":1, 46 | "URI":"", 47 | "vSizeRGB":[1600,1200], 48 | "bRGB":1, 49 | "bDepth":0, 50 | "btRGB":0, 51 | "btDepth":0, 52 | "bIR":0, 53 | "/_SHMrgb":"SHMvzRGB", 54 | } 55 | -------------------------------------------------------------------------------- /kiss/_WebSocket.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":10, 16 | } 17 | "vBASE":["wsServer"], 18 | } 19 | 20 | { 21 | "name":"ws", 22 | "class":"_WebSocket", 23 | "bON":1, 24 | "bLog":1, 25 | { 26 | "name":"thread", 27 | "FPS":1, 28 | } 29 | "nPacket":1024, 30 | "nPbuffer":512, 31 | } 32 | 33 | { 34 | "name":"wsServer", 35 | "class":"_WebSocketServer", 36 | "bON":1, 37 | "bLog":1, 38 | { 39 | "name":"thread", 40 | "FPS":10, 41 | } 42 | "host":"localhost", 43 | "port":7890, 44 | "tOutMs":10000, 45 | } 46 | -------------------------------------------------------------------------------- /kiss/_Xbee.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":10, 16 | } 17 | "vBASE":["xb1"], 18 | } 19 | 20 | { 21 | "name":"serial1", 22 | "class":"_SerialPort", 23 | "bON":1, 24 | { 25 | "name":"thread", 26 | "FPS":30, 27 | } 28 | { 29 | "name":"threadR", 30 | "FPS":30, 31 | "vRunThread":["xb1.threadR"], 32 | } 33 | "port":"/dev/ttyUSB0", 34 | "baud":57600, 35 | } 36 | 37 | { 38 | "name":"xb1", 39 | "class":"_Xbee", 40 | "bON":1, 41 | "bLog":1, 42 | { 43 | "name":"thread", 44 | "FPS":1, 45 | } 46 | { 47 | "name":"threadR", 48 | "FPS":30, 49 | } 50 | "_IObase":"serial1", 51 | "/destAddr":"000000000000FFFF", /* Broadcast address*/ 52 | "destAddr":"0013A200421D6C8F", 53 | } 54 | -------------------------------------------------------------------------------- /kiss/_xArm.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bWindow":0, 6 | "bDraw":1, 7 | "bLog":1, 8 | "bStdErr":0, 9 | "waitKey":30, 10 | } 11 | 12 | { 13 | "name":"OpenKAI", 14 | "class":"_Console", 15 | "bON":1, 16 | } 17 | 18 | { 19 | "name":"OKview", 20 | "class":"Window", 21 | "bON":0, 22 | "bFullScreen":0, 23 | } 24 | 25 | { 26 | "name":"xArm", 27 | "class":"_xArm", 28 | "bON":1, 29 | "FPS":30, 30 | "bLog":1, 31 | "Window":"OKview", 32 | "_Console":"OpenKAI", 33 | "ip":"192.168.1.222", 34 | "oprMode":3, 35 | } 36 | -------------------------------------------------------------------------------- /kiss/test/WebSocket.kiss: -------------------------------------------------------------------------------- 1 | { 2 | "name":"APP", 3 | "class":"Startup", 4 | "appName":"OpenKAI", 5 | "bLog":1, 6 | "bStdErr":1, 7 | } 8 | 9 | { 10 | "name":"console", 11 | "class":"_Console", 12 | "bON":0, 13 | { 14 | "name":"thread", 15 | "FPS":10, 16 | } 17 | "vBASE":["wsServer"], 18 | } 19 | 20 | { 21 | "name":"wsServer", 22 | "class":"_WebSocketServer", 23 | "bON":1, 24 | "bLog":1, 25 | { 26 | "name":"thread", 27 | "FPS":10, 28 | } 29 | "wsMode":3, 30 | "host":"localhost", 31 | "port":7890, 32 | "tOutMs":10000, 33 | } 34 | 35 | { 36 | "name":"TestWebSocket", 37 | "class":"_TestWebSocket", 38 | "bON":1, 39 | "bLog":1, 40 | { 41 | "name":"thread", 42 | "FPS":1, 43 | } 44 | "_WebSocketServer":"wsServer", 45 | } 46 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | OpenKAI 5 | 0.0.0 6 | TODO: Package description 7 | kai 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /sh/Config/usbReset.sh: -------------------------------------------------------------------------------- 1 | sudo uhubctl -a cycle -l 1 -p 1-4 2 | sudo uhubctl -a cycle -l 2 -p 1-4 3 | -------------------------------------------------------------------------------- /sh/Deploy/gsvLidars.sh: -------------------------------------------------------------------------------- 1 | /home/lab/dev/OpenKAI/build/OpenKAI /home/lab/dev/OpenKAI/kiss/app/GSV/GSV_lidar_1.kiss & 2 | /home/lab/dev/OpenKAI/build/OpenKAI /home/lab/dev/OpenKAI/kiss/app/GSV/GSV_lidar_2.kiss & 3 | /home/lab/dev/OpenKAI/build/OpenKAI /home/lab/dev/OpenKAI/kiss/app/GSV/GSV_lidar_3.kiss & 4 | /home/lab/dev/OpenKAI/build/OpenKAI /home/lab/dev/OpenKAI/kiss/app/GSV/GSV_lidar_4.kiss & 5 | /home/lab/dev/OpenKAI/build/OpenKAI /home/lab/dev/OpenKAI/kiss/app/GSV/GSV_lidar_5.kiss & 6 | /home/lab/dev/OpenKAI/build/OpenKAI /home/lab/dev/OpenKAI/kiss/app/GSV/GSV_lidar_6.kiss & 7 | 8 | -------------------------------------------------------------------------------- /sh/Gstreamer/gst.sh: -------------------------------------------------------------------------------- 1 | gst-launch-1.0 udpsrc port=5678 multicast-group=224.1.1.1 ! application/x-rtp, payload=96 ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false async=false 2 | 3 | -------------------------------------------------------------------------------- /sh/Gstreamer/gstSend.sh: -------------------------------------------------------------------------------- 1 | gst-launch-1.0 v4l2src device=/dev/video0 ! videoflip method=0 ! video/x-raw,format=BGR,width=1280,height=720,framerate=30/1 ! videoconvert ! omxh264enc insert-sps-pps=true ! rtph264pay mtu=1400 config-interval=1 pt=96 ! udpsink host=224.1.1.1 port=5678 auto-multicast=true 2 | 3 | gst-launch-1.0 libcamerasrc ! video/x-raw,format=RGB,width=1280,height=720,framerate=30/1 ! v4l2convert ! v4l2h264enc ! 'video/x-h264,level=(string)4' ! h264parse ! tee name=t t. ! queue ! matroskamux ! filesink location=/home/pi/video/test.mkv t. ! queue ! rtph264pay mtu=1400 config-interval=1 pt=96 ! udpsink host=192.168.1.245 port=5678 auto-multicast=false 4 | 5 | gst-launch-1.0 libcamerasrc ! video/x-raw,format=RGB,width=1280,height=720,framerate=30/1 ! v4l2convert ! v4l2h264enc ! 'video/x-h264,level=(string)4' ! h264parse ! rtph264pay mtu=1400 config-interval=1 pt=96 ! udpsink host=192.168.1.245 port=5678 auto-multicast=false 6 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/NVMeBoot/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Jetsonhacks 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/NVMeBoot/copy-rootfs-ssd.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Mount the SSD as /mnt 3 | sudo mount /dev/nvme0n1p1 /mnt 4 | # Copy over the rootfs from the SD card to the SSD 5 | sudo rsync -axHAWX --numeric-ids --info=progress2 --exclude={"/dev/","/proc/","/sys/","/tmp/","/run/","/mnt/","/media/*","/lost+found"} / /mnt 6 | # We want to keep the SSD mounted for further operations 7 | # So we do not unmount the SSD 8 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/NVMeBoot/data/setssdroot.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Change rootfs to SSD in M.2 key M slot (nvme0n1p1) 3 | DefaultDependencies=no 4 | Conflicts=shutdown.target 5 | After=systemd-remount-fs.service 6 | Before=local-fs-pre.target local-fs.target shutdown.target 7 | Wants=local-fs-pre.target 8 | ConditionPathExists=/dev/nvme0n1p1 9 | ConditionPathExists=/etc/setssdroot.conf 10 | ConditionVirtualization=!container 11 | [Service] 12 | Type=oneshot 13 | RemainAfterExit=yes 14 | ExecStart=/sbin/setssdroot.sh 15 | [Install] 16 | WantedBy=default.target 17 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/NVMeBoot/data/setssdroot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Runs at startup, switches rootfs to the SSD on nvme0 (M.2 Key M slot) 3 | NVME_DRIVE="/dev/nvme0n1p1" 4 | CHROOT_PATH="/nvmeroot" 5 | 6 | INITBIN=/lib/systemd/systemd 7 | EXT4_OPT="-o defaults -o errors=remount-ro -o discard" 8 | 9 | modprobe ext4 10 | 11 | mkdir -p ${CHROOT_PATH} 12 | mount -t ext4 ${EXT4_OPT} ${NVME_DRIVE} ${CHROOT_PATH} 13 | 14 | cd ${CHROOT_PATH} 15 | /bin/systemctl --no-block switch-root ${CHROOT_PATH} 16 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/NVMeBoot/setup-service.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Setup the service to set the rootfs to point to the SSD 3 | sudo cp data/setssdroot.service /etc/systemd/system 4 | sudo cp data/setssdroot.sh /sbin 5 | sudo chmod 777 /sbin/setssdroot.sh 6 | systemctl daemon-reload 7 | sudo systemctl enable setssdroot.service 8 | 9 | # Copy these over to the SSD 10 | sudo cp /etc/systemd/system/setssdroot.service /mnt/etc/systemd/system/setssdroot.service 11 | sudo cp /sbin/setssdroot.sh /mnt/sbin/setssdroot.sh 12 | 13 | # Create setssdroot.conf which tells the service script to set the rootfs to the SSD 14 | # If you want to boot from SD again, remove the file /etc/setssdroot.conf from the SD card. 15 | # touch creates an empty file 16 | sudo touch /etc/setssdroot.conf 17 | echo 'Service to set the rootfs to the SSD installed.' 18 | echo 'Make sure that you have copied the rootfs to SSD.' 19 | echo 'Reboot for changes to take effect.' 20 | 21 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/SDboot/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Jetsonhacks 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/SDboot/copy-rootfs-tf.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Mount the SSD as /mnt 3 | #sudo mount /dev/nvme0n1p1 /mnt 4 | sudo mount /dev/mmcblk1p1 /mnt 5 | # Copy over the rootfs from the SD card to the SSD 6 | sudo rsync -axHAWX --numeric-ids --info=progress2 --exclude={"/dev/","/proc/","/sys/","/tmp/","/run/","/mnt/","/media/*","/lost+found"} / /mnt 7 | # We want to keep the SSD mounted for further operations 8 | # So we do not unmount the SSD 9 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/SDboot/data/setssdroot.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Change rootfs to SSD in M.2 key M slot (nvme0n1p1) 3 | DefaultDependencies=no 4 | Conflicts=shutdown.target 5 | After=systemd-remount-fs.service 6 | Before=local-fs-pre.target local-fs.target shutdown.target 7 | Wants=local-fs-pre.target 8 | ConditionPathExists=/dev/mmcblk1p1 9 | ConditionPathExists=/etc/setssdroot.conf 10 | ConditionVirtualization=!container 11 | [Service] 12 | Type=oneshot 13 | RemainAfterExit=yes 14 | ExecStart=/sbin/setssdroot.sh 15 | [Install] 16 | WantedBy=default.target 17 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/SDboot/data/setssdroot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Runs at startup, switches rootfs to the SSD on nvme0 (M.2 Key M slot) 3 | NVME_DRIVE="/dev/mmcblk1p1" 4 | CHROOT_PATH="/nvmeroot" 5 | 6 | INITBIN=/lib/systemd/systemd 7 | EXT4_OPT="-o defaults -o errors=remount-ro -o discard" 8 | 9 | modprobe ext4 10 | 11 | mkdir -p ${CHROOT_PATH} 12 | mount -t ext4 ${EXT4_OPT} ${NVME_DRIVE} ${CHROOT_PATH} 13 | 14 | cd ${CHROOT_PATH} 15 | /bin/systemctl --no-block switch-root ${CHROOT_PATH} 16 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/SDboot/setup-service.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Setup the service to set the rootfs to point to the SSD 3 | sudo cp data/setssdroot.service /etc/systemd/system 4 | sudo cp data/setssdroot.sh /sbin 5 | sudo chmod 777 /sbin/setssdroot.sh 6 | systemctl daemon-reload 7 | sudo systemctl enable setssdroot.service 8 | 9 | # Copy these over to the SSD 10 | sudo cp /etc/systemd/system/setssdroot.service /mnt/etc/systemd/system/setssdroot.service 11 | sudo cp /sbin/setssdroot.sh /mnt/sbin/setssdroot.sh 12 | 13 | # Create setssdroot.conf which tells the service script to set the rootfs to the SSD 14 | # If you want to boot from SD again, remove the file /etc/setssdroot.conf from the SD card. 15 | # touch creates an empty file 16 | sudo touch /etc/setssdroot.conf 17 | echo 'Service to set the rootfs to the TF installed.' 18 | echo 'Make sure that you have copied the rootfs to TF.' 19 | echo 'Reboot for changes to take effect.' 20 | 21 | -------------------------------------------------------------------------------- /sh/Jetson/setBoot/auto_partition.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | sudo parted /dev/nvme0n1 mklabel gpt 3 | sudo parted /dev/nvme0n1 mkpart logical 0 100% 4 | sudo mkfs.ext4 /dev/nvme0n1p1 5 | -------------------------------------------------------------------------------- /sh/RaspberryPi/CM4_dt_blob.7z: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/sh/RaspberryPi/CM4_dt_blob.7z -------------------------------------------------------------------------------- /sh/RaspberryPi/ok.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=ARmeasure 3 | 4 | [Service] 5 | Environment=DISPLAY=:0.0 6 | Environment=XAUTHORITY=/home/pi/.Xauthority 7 | ExecStart=/home/pi/ok.sh 8 | Restart=always 9 | RestartSec=2s 10 | KillMode=process 11 | TimeoutSec=infinity 12 | 13 | [Install] 14 | WantedBy=multi-user.target 15 | -------------------------------------------------------------------------------- /sh/Setup/ardupilot_dev_pc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | git clone --depth 1 https://github.com/ArduPilot/ardupilot.git 4 | cd ardupilot 5 | git submodule update --init --recursive 6 | 7 | Tools/environment_install/install-prereqs-ubuntu.sh -y 8 | . ~/.profile 9 | 10 | ./waf configure --board sitl 11 | ./waf copter 12 | cd ArduCopter 13 | 14 | ./Tools/autotest/sim_vehicle.py -v ArduCopter -f quad --map --console 15 | nano ardupilot/Tools/autotest/locations.txt 16 | Okushiga=36.779180,138.527999,1500,0 17 | /home/kai/dev/ardupilot/Tools/autotest/sim_vehicle.py -v ArduCopter -f quad -L Okushiga --map --console --count=1 --add-param-file=/home/kai/ap_sitl_copter.parm 18 | 19 | ./waf configure --board CubeBlack 20 | ./waf copter 21 | ./waf --targets bin/arducopter --upload 22 | -------------------------------------------------------------------------------- /sh/Setup/openCL.sh: -------------------------------------------------------------------------------- 1 | #---------------------------------------------------- 2 | # Intel 3 | 4 | #https://www.intel.com/content/www/us/en/developer/articles/tool/opencl-drivers.html 5 | https://github.com/intel/compute-runtime/releases 6 | 7 | sudo apt-get install clinfo 8 | 9 | -------------------------------------------------------------------------------- /src/3D/Mesh/_MeshStream.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _MeshStream.h 3 | * 4 | * Created on: May 24, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_Mesh__MeshStream_H_ 9 | #define OpenKAI_src_3D_Mesh__MeshStream_H_ 10 | 11 | #include "../_GeometryBase.h" 12 | 13 | namespace kai 14 | { 15 | class _MeshStream : public _GeometryBase 16 | { 17 | public: 18 | _MeshStream(); 19 | virtual ~_MeshStream(); 20 | 21 | virtual int init(void *pKiss); 22 | virtual int check(void); 23 | 24 | virtual void setAccept(bool b); 25 | virtual int nP(void); 26 | virtual int iP(void); 27 | 28 | virtual int addVertex(const Vector3d &vP, uint64_t tStamp = UINT64_MAX); 29 | virtual int addTriangle(const vInt3& vVertices, uint64_t tStamp = UINT64_MAX); 30 | 31 | virtual void startStream(void); 32 | virtual void stopStream(void); 33 | virtual void clear(void); 34 | 35 | protected: 36 | virtual void getStream(void *p); 37 | virtual void getNextFrame(void *p); 38 | virtual void getLattice(void *p); 39 | 40 | public: 41 | GEOMETRY_POINT *m_pP; 42 | int m_nP; 43 | int m_iP; 44 | uint64_t m_tLastUpdate; 45 | bool m_bAccept; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /src/3D/PointCloud/PCfilter/_PCdownSample.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCdownSample.cpp 3 | * 4 | * Created on: Feb 7, 2021 5 | * Author: yankai 6 | */ 7 | 8 | #include "_PCdownSample.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _PCdownSample::_PCdownSample() 14 | { 15 | m_rVoxel = 0.1; 16 | } 17 | 18 | _PCdownSample::~_PCdownSample() 19 | { 20 | } 21 | 22 | int _PCdownSample::init(void *pKiss) 23 | { 24 | CHECK_(_GeometryBase::init(pKiss)); 25 | Kiss *pK = (Kiss *)pKiss; 26 | 27 | pK->v("rVoxel", &m_rVoxel); 28 | 29 | return OK_OK; 30 | } 31 | 32 | int _PCdownSample::start(void) 33 | { 34 | NULL__(m_pT, OK_ERR_NULLPTR); 35 | return m_pT->start(getUpdate, this); 36 | } 37 | 38 | int _PCdownSample::check(void) 39 | { 40 | // NULL__(m_pInCtx.m_pPCB, -1); 41 | 42 | return _GeometryBase::check(); 43 | } 44 | 45 | void _PCdownSample::update(void) 46 | { 47 | while (m_pT->bAlive()) 48 | { 49 | m_pT->autoFPS(); 50 | 51 | updateFilter(); 52 | 53 | } 54 | } 55 | 56 | void _PCdownSample::updateFilter(void) 57 | { 58 | IF_(check() != OK_OK); 59 | 60 | // PointCloud* pOut = m_sPC.next(); 61 | // PointCloud pcIn; 62 | // m_pPCB->getPC(&pcIn); 63 | 64 | // *pOut = *pcIn.VoxelDownSample(m_rVoxel); 65 | } 66 | 67 | } 68 | -------------------------------------------------------------------------------- /src/3D/PointCloud/PCfilter/_PCdownSample.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCdownSample.h 3 | * 4 | * Created on: Feb 7, 2021 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCdownSample_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCdownSample_H_ 10 | 11 | #include "../../_GeometryBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _PCdownSample : public _GeometryBase 17 | { 18 | public: 19 | _PCdownSample(); 20 | virtual ~_PCdownSample(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | int check(void); 25 | 26 | private: 27 | void update(void); 28 | void updateFilter(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_PCdownSample *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | public: 36 | float m_rVoxel; 37 | }; 38 | 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /src/3D/PointCloud/PCfilter/_PCremove.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCremove.cpp 3 | * 4 | * Created on: Sept 3, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #include "_PCremove.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _PCremove::_PCremove() 14 | { 15 | m_nP = 16; 16 | m_r = 0.05; 17 | } 18 | 19 | _PCremove::~_PCremove() 20 | { 21 | } 22 | 23 | int _PCremove::init(void *pKiss) 24 | { 25 | CHECK_(_GeometryBase::init(pKiss)); 26 | Kiss *pK = (Kiss *)pKiss; 27 | 28 | pK->v("nP", &m_nP); 29 | pK->v("r", &m_r); 30 | 31 | return OK_OK; 32 | } 33 | 34 | int _PCremove::start(void) 35 | { 36 | NULL__(m_pT, OK_ERR_NULLPTR); 37 | return m_pT->start(getUpdate, this); 38 | } 39 | 40 | int _PCremove::check(void) 41 | { 42 | // NULL__(m_pInCtx.m_pPCB, -1); 43 | 44 | return _GeometryBase::check(); 45 | } 46 | 47 | void _PCremove::update(void) 48 | { 49 | while (m_pT->bAlive()) 50 | { 51 | m_pT->autoFPS(); 52 | 53 | updateFilter(); 54 | 55 | } 56 | } 57 | 58 | void _PCremove::updateFilter(void) 59 | { 60 | IF_(check() != OK_OK); 61 | 62 | // PointCloud pcIn; 63 | // m_pPCB->getPC(&pcIn); 64 | 65 | // auto[pc, vP] = pcIn.RemoveStatisticalOutliers(20,2.0);//m_nP, m_r); 66 | 67 | // PointCloud* pOut = m_sPC.next(); 68 | // *pOut = *pc; 69 | } 70 | 71 | } 72 | -------------------------------------------------------------------------------- /src/3D/PointCloud/PCfilter/_PCremove.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCremove.h 3 | * 4 | * Created on: Sept 3, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCremove_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCremove_H_ 10 | 11 | #include "../../_GeometryBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _PCremove : public _GeometryBase 17 | { 18 | public: 19 | _PCremove(); 20 | virtual ~_PCremove(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | int check(void); 25 | 26 | private: 27 | void update(void); 28 | void updateFilter(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_PCremove *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | public: 36 | int m_nP; 37 | double m_r; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/3D/PointCloud/PCfilter/_PCtransform.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCtransform.h 3 | * 4 | * Created on: Sept 3, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCtransform_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCtransform_H_ 10 | 11 | #include "../_PCframe.h" 12 | #include "../../../IO/_File.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _PCtransform : public _PCframe 18 | { 19 | public: 20 | _PCtransform(); 21 | virtual ~_PCtransform(); 22 | 23 | int init(void *pKiss); 24 | int start(void); 25 | int check(void); 26 | 27 | virtual void setTranslationMatrix(Matrix4d_u &mTt); 28 | virtual void saveParamKiss(void); 29 | 30 | private: 31 | void updateTransform(void); 32 | void update(void); 33 | static void *getUpdate(void *This) 34 | { 35 | ((_PCtransform *)This)->update(); 36 | return NULL; 37 | } 38 | 39 | public: 40 | Matrix4d_u m_mTt; 41 | string m_paramKiss; 42 | }; 43 | 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /src/3D/PointCloud/PCregistration/_PCregistGlobal.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCregistGlobal.h 3 | * 4 | * Created on: Sept 6, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCregistGlobal_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCregistGlobal_H_ 10 | 11 | #include "../PCfilter/_PCtransform.h" 12 | #include 13 | using namespace open3d::pipelines::registration; 14 | 15 | namespace kai 16 | { 17 | 18 | class _PCregistGlobal: public _ModuleBase 19 | { 20 | public: 21 | _PCregistGlobal(); 22 | virtual ~_PCregistGlobal(); 23 | 24 | int init(void* pKiss); 25 | int start(void); 26 | int check(void); 27 | void console(void* pConsole); 28 | 29 | private: 30 | std::shared_ptr preprocess(PointCloud& pc); 31 | void updateRegistration(void); 32 | void update(void); 33 | static void* getUpdate(void* This) 34 | { 35 | (( _PCregistGlobal *) This)->update(); 36 | return NULL; 37 | } 38 | 39 | public: 40 | double m_rNormal; 41 | double m_rFeature; 42 | int m_maxNNnormal; 43 | int m_maxNNfpfh; 44 | 45 | _PCframe* m_pSrc; 46 | _PCframe* m_pTgt; 47 | RegistrationResult m_RR; 48 | _PCtransform* m_pTf; 49 | double m_lastFit; 50 | }; 51 | 52 | } 53 | #endif 54 | -------------------------------------------------------------------------------- /src/3D/PointCloud/PCregistration/_PCregistICP.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCregistICP.h 3 | * 4 | * Created on: Sept 6, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCregistICP_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCregistICP_H_ 10 | 11 | #include "../PCfilter/_PCtransform.h" 12 | using namespace open3d::pipelines::registration; 13 | 14 | namespace kai 15 | { 16 | 17 | enum PCREGIST_ICP_EST 18 | { 19 | icp_p2point = 0, 20 | icp_p2plane = 1, 21 | }; 22 | 23 | class _PCregistICP : public _ModuleBase 24 | { 25 | public: 26 | _PCregistICP(); 27 | virtual ~_PCregistICP(); 28 | 29 | int init(void *pKiss); 30 | int start(void); 31 | int check(void); 32 | void console(void *pConsole); 33 | 34 | private: 35 | void updateRegistration(void); 36 | void update(void); 37 | static void *getUpdate(void *This) 38 | { 39 | ((_PCregistICP *)This)->update(); 40 | return NULL; 41 | } 42 | 43 | public: 44 | float m_thr; // ICP threshold 45 | PCREGIST_ICP_EST m_est; 46 | _PCframe *m_pSrc; 47 | _PCframe *m_pTgt; 48 | RegistrationResult m_RR; 49 | _PCtransform *m_pTf; 50 | double m_lastFit; 51 | }; 52 | 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /src/3D/PointCloud/_PCfile.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCfile.cpp 3 | * 4 | * Created on: Sept 3, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #include "_PCfile.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _PCfile::_PCfile() 14 | { 15 | } 16 | 17 | _PCfile::~_PCfile() 18 | { 19 | } 20 | 21 | int _PCfile::init(void *pKiss) 22 | { 23 | CHECK_(_PCframe::init(pKiss)); 24 | Kiss *pK = (Kiss *)pKiss; 25 | 26 | pK->a("vfName", &m_vfName); 27 | open(); 28 | 29 | return OK_OK; 30 | } 31 | 32 | bool _PCfile::open(void) 33 | { 34 | IF_F(m_vfName.empty()); 35 | 36 | m_pcl.Clear(); 37 | PointCloud pc; 38 | for (string f : m_vfName) 39 | { 40 | pc.Clear(); 41 | // io::ReadPointCloudOption ro; 42 | IF_CONT(!io::ReadPointCloud(f, pc)); 43 | m_pcl += pc; 44 | LOG_I("File: " + f + ", Npoints: " + i2str(pc.points_.size())); 45 | } 46 | 47 | return true; 48 | } 49 | 50 | int _PCfile::start(void) 51 | { 52 | NULL__(m_pT, OK_ERR_NULLPTR); 53 | return m_pT->start(getUpdate, this); 54 | } 55 | 56 | void _PCfile::update(void) 57 | { 58 | while (m_pT->bAlive()) 59 | { 60 | m_pT->autoFPS(); 61 | 62 | PointCloud* pNext = m_sPC.next(); 63 | pNext->Clear(); 64 | *pNext = m_pcl; 65 | pNext->Transform(m_mT); 66 | swapBuffer(); 67 | 68 | writeSharedMem(); 69 | } 70 | } 71 | 72 | } 73 | -------------------------------------------------------------------------------- /src/3D/PointCloud/_PCfile.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCfile.h 3 | * 4 | * Created on: Sept 3, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCfile_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCfile_H_ 10 | 11 | #include "_PCframe.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _PCfile : public _PCframe 17 | { 18 | public: 19 | _PCfile(); 20 | virtual ~_PCfile(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | 26 | private: 27 | void update(void); 28 | static void *getUpdate(void *This) 29 | { 30 | ((_PCfile *)This)->update(); 31 | return NULL; 32 | } 33 | 34 | public: 35 | vector m_vfName; 36 | PointCloud m_pcl; 37 | }; 38 | 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /src/3D/PointCloud/_PCmerge.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCmerge.h 3 | * 4 | * Created on: May 24, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCmerge_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCmerge_H_ 10 | 11 | #include "../../Base/common.h" 12 | #include "../_GeometryBase.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _PCmerge : public _GeometryBase 18 | { 19 | public: 20 | _PCmerge(); 21 | virtual ~_PCmerge(); 22 | 23 | int init(void *pKiss); 24 | int start(void); 25 | int check(void); 26 | 27 | private: 28 | void updateMerge(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_PCmerge *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | vector<_GeometryBase *> m_vpGB; 38 | float m_rVoxel; 39 | }; 40 | 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /src/3D/PointCloud/_PCrecv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCrecv.h 3 | * 4 | * Created on: Oct 8, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCrecv_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCrecv_H_ 10 | 11 | #include "_PCsend.h" 12 | #include "_PCstream.h" 13 | #include "../../Protocol/_ProtocolBase.h" 14 | 15 | #define PC_N_BUF 1024 16 | 17 | namespace kai 18 | { 19 | class _PCrecv : public _PCstream 20 | { 21 | public: 22 | _PCrecv(); 23 | virtual ~_PCrecv(); 24 | 25 | virtual int init(void *pKiss); 26 | virtual int link(void); 27 | virtual int start(void); 28 | virtual int check(void); 29 | 30 | protected: 31 | virtual bool readCMD(PROTOCOL_CMD *pCmd); 32 | virtual void handleCMD(const PROTOCOL_CMD &cmd); 33 | virtual void decodeStream(const PROTOCOL_CMD &cmd); 34 | 35 | private: 36 | void update(void); 37 | static void *getUpdate(void *This) 38 | { 39 | ((_PCrecv *)This)->update(); 40 | return NULL; 41 | } 42 | 43 | protected: 44 | _IObase *m_pIO; 45 | uint64_t m_nCMDrecv; 46 | 47 | uint8_t m_pBuf[PC_N_BUF]; 48 | int m_nRead; 49 | int m_iRead; 50 | }; 51 | 52 | } 53 | #endif 54 | -------------------------------------------------------------------------------- /src/3D/PointCloud/_PCsend.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _PCsend.h 3 | * 4 | * Created on: Oct 8, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_3D_PointCloud_PCsend_H_ 9 | #define OpenKAI_src_3D_PointCloud_PCsend_H_ 10 | 11 | #include "../../Base/common.h" 12 | #include "../../IO/_IObase.h" 13 | #include "../../Protocol/_ProtocolBase.h" 14 | #include "../_GeometryBase.h" 15 | 16 | #define PC_N_HDR 4 17 | #define PC_STREAM 0 18 | #define PC_FRAME_END 1 19 | 20 | namespace kai 21 | { 22 | 23 | class _PCsend : public _GeometryBase 24 | { 25 | public: 26 | _PCsend(); 27 | virtual ~_PCsend(); 28 | 29 | int init(void *pKiss); 30 | int start(void); 31 | int check(void); 32 | 33 | private: 34 | void sendPC(void); 35 | void update(void); 36 | static void *getUpdate(void *This) 37 | { 38 | ((_PCsend *)This)->update(); 39 | return NULL; 40 | } 41 | 42 | public: 43 | _IObase *m_pIO; 44 | 45 | int m_iPsent; 46 | 47 | uint8_t *m_pB; 48 | int m_nB; 49 | uint64_t m_tInt; 50 | }; 51 | 52 | } 53 | #endif 54 | -------------------------------------------------------------------------------- /src/Actuator/Articulated/_xArm.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Actuator_Articulated__xArm_H_ 2 | #define OpenKAI_src_Actuator_Articulated__xArm_H_ 3 | 4 | #include "../_ActuatorBase.h" 5 | #include 6 | 7 | namespace kai 8 | { 9 | 10 | class _xArm : public _ActuatorBase 11 | { 12 | public: 13 | _xArm(); 14 | ~_xArm(); 15 | 16 | virtual int init(void *pKiss); 17 | virtual int start(void); 18 | virtual int check(void); 19 | 20 | virtual bool power(bool bON); 21 | 22 | protected: 23 | virtual void updatePos(void); 24 | virtual void gotoPos(vFloat3 &vP); 25 | virtual vFloat3 getPtarget(void); 26 | virtual vFloat3 getP(void); 27 | virtual vFloat3 getAtarget(void); 28 | virtual vFloat3 getA(void); 29 | virtual void readState(void); 30 | 31 | void update(void); 32 | static void *getUpdate(void *This) 33 | { 34 | ((_xArm *)This)->update(); 35 | return NULL; 36 | } 37 | 38 | public: 39 | XArmAPI *m_pArm; 40 | 41 | string m_ip; 42 | int m_mode; 43 | int m_state; 44 | }; 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /src/Actuator/Motor/_DDSM.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Actuator__DDSM_H_ 2 | #define OpenKAI_src_Actuator__DDSM_H_ 3 | 4 | #include "../_ActuatorBase.h" 5 | #include "../../IO/_IObase.h" 6 | #include "../../Utility/util.h" 7 | 8 | #define DDSM_CMD_NB 10 9 | 10 | namespace kai 11 | { 12 | 13 | class _DDSM : public _ActuatorBase 14 | { 15 | public: 16 | _DDSM(); 17 | ~_DDSM(); 18 | 19 | virtual int init(void *pKiss); 20 | virtual int link(void); 21 | virtual int start(void); 22 | virtual int check(void); 23 | virtual void console(void *pConsole); 24 | 25 | private: 26 | bool setID(int iChan = 0); 27 | bool setMode(int iChan = 0); 28 | bool setSpeed(int iChan = 0); 29 | bool readCMD(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_DDSM *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | void updateR(void); 38 | static void *getUpdateR(void *This) 39 | { 40 | ((_DDSM *)This)->updateR(); 41 | return NULL; 42 | } 43 | 44 | private: 45 | _Thread *m_pTr; 46 | _IObase *m_pIO; 47 | 48 | // uint8_t m_iMode; 49 | }; 50 | 51 | } 52 | #endif 53 | -------------------------------------------------------------------------------- /src/Actuator/Motor/_ZDmotor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ZDmotor.h 3 | * 4 | * Created on: Sept 21, 2022 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Actuator__ZDmotor_H_ 9 | #define OpenKAI_src_Actuator__ZDmotor_H_ 10 | 11 | #include "../../Protocol/_Modbus.h" 12 | #include "../_ActuatorBase.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _ZDmotor : public _ActuatorBase 18 | { 19 | public: 20 | _ZDmotor(); 21 | ~_ZDmotor(); 22 | 23 | int init(void *pKiss); 24 | int link(void); 25 | int start(void); 26 | int check(void); 27 | 28 | private: 29 | bool setPower(bool bON); 30 | bool setMode(void); 31 | bool setSpeed(void); 32 | bool setAccel(void); 33 | bool setBrake(void); 34 | 35 | bool bComplete(void); 36 | bool stopMove(void); 37 | bool setSlave(int iSlave); 38 | bool readStatus(void); 39 | bool clearAlarm(void); 40 | 41 | void setup(void); 42 | 43 | void updateMove(void); 44 | void update(void); 45 | static void *getUpdate(void *This) 46 | { 47 | ((_ZDmotor *)This)->update(); 48 | return NULL; 49 | } 50 | 51 | private: 52 | _Modbus *m_pMB; 53 | int m_iSlave; 54 | int m_iMode; 55 | 56 | INTERVAL_EVENT m_ieReadStatus; 57 | }; 58 | 59 | } 60 | #endif 61 | -------------------------------------------------------------------------------- /src/Actuator/Motor/_ZLAC8015.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ZLAC8015.h 3 | * 4 | * Created on: June 22, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Actuator__ZLAC8015_H_ 9 | #define OpenKAI_src_Actuator__ZLAC8015_H_ 10 | 11 | #include "../../Protocol/_Modbus.h" 12 | #include "../_ActuatorBase.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _ZLAC8015 : public _ActuatorBase 18 | { 19 | public: 20 | _ZLAC8015(); 21 | ~_ZLAC8015(); 22 | 23 | int init(void *pKiss); 24 | int link(void); 25 | int start(void); 26 | int check(void); 27 | 28 | private: 29 | // bool setPower(bool bON); 30 | bool setMode(void); 31 | bool setSpeed(void); 32 | bool setAccel(void); 33 | bool setBrake(void); 34 | 35 | bool bComplete(void); 36 | bool stopMove(void); 37 | bool setSlave(int iSlave); 38 | bool readStatus(void); 39 | bool clearAlarm(void); 40 | 41 | void setup(void); 42 | 43 | void updateMove(void); 44 | void update(void); 45 | static void *getUpdate(void *This) 46 | { 47 | ((_ZLAC8015 *)This)->update(); 48 | return NULL; 49 | } 50 | 51 | private: 52 | _Modbus *m_pMB; 53 | int m_iSlave; 54 | int m_iMode; 55 | 56 | INTERVAL_EVENT m_ieReadStatus; 57 | }; 58 | 59 | } 60 | #endif 61 | -------------------------------------------------------------------------------- /src/Actuator/Motor/_ZLAC8015D.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ZLAC8015D.h 3 | * 4 | * Created on: June 22, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Actuator__ZLAC8015D_H_ 9 | #define OpenKAI_src_Actuator__ZLAC8015D_H_ 10 | 11 | #include "../../Protocol/_Modbus.h" 12 | #include "../_ActuatorBase.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _ZLAC8015D : public _ActuatorBase 18 | { 19 | public: 20 | _ZLAC8015D(); 21 | ~_ZLAC8015D(); 22 | 23 | int init(void *pKiss); 24 | int link(void); 25 | int start(void); 26 | int check(void); 27 | 28 | private: 29 | bool setMode(void); 30 | bool setSpeed(void); 31 | bool setAccel(void); 32 | bool setBrake(void); 33 | 34 | bool bComplete(void); 35 | bool stopMove(void); 36 | bool setSlave(int iSlave); 37 | bool readStatus(void); 38 | bool clearAlarm(void); 39 | 40 | void setup(void); 41 | 42 | void updateMove(void); 43 | void update(void); 44 | static void *getUpdate(void *This) 45 | { 46 | ((_ZLAC8015D *)This)->update(); 47 | return NULL; 48 | } 49 | 50 | private: 51 | _Modbus *m_pMB; 52 | int m_iSlave; 53 | int m_iMode; 54 | 55 | INTERVAL_EVENT m_ieReadStatus; 56 | }; 57 | 58 | } 59 | #endif 60 | -------------------------------------------------------------------------------- /src/Actuator/_Feetech.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Feetech.h 3 | * 4 | * Created on: Jan 6, 2024 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Actuator__Feetech_H_ 9 | #define OpenKAI_src_Actuator__Feetech_H_ 10 | 11 | #include "_ActuatorBase.h" 12 | #include "../Dependencies/Feetech/SCServo.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _Feetech : public _ActuatorBase 18 | { 19 | public: 20 | _Feetech(); 21 | ~_Feetech(); 22 | 23 | virtual int init(void *pKiss); 24 | virtual int link(void); 25 | virtual int check(void); 26 | virtual int start(void); 27 | virtual void console(void *pConsole); 28 | 29 | protected: 30 | bool open(void); 31 | void close(void); 32 | 33 | bool setID(uint16_t iSlave); 34 | bool setBaudrate(uint32_t baudrate); 35 | bool saveData(void); 36 | 37 | bool setPos(void); 38 | bool setSpeed(void); 39 | bool setAccel(void); 40 | 41 | bool readStatus(void); 42 | void updateMove(void); 43 | void update(void); 44 | static void *getUpdate(void *This) 45 | { 46 | ((_Feetech *)This)->update(); 47 | return NULL; 48 | } 49 | 50 | public: 51 | string m_port; 52 | int m_baud; 53 | bool m_bOpen; 54 | SMSBL m_servo; 55 | uint8_t m_ID; 56 | ACTUATOR_AXIS *m_pA; 57 | 58 | INTERVAL_EVENT m_ieReadStatus; 59 | }; 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /src/Actuator/_MultiActuatorsBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _MultiActuatorsBase.h 3 | * 4 | * Created on: May 16, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Actuator__MultiActuatorsBase_H_ 9 | #define OpenKAI_src_Actuator__MultiActuatorsBase_H_ 10 | 11 | #include "_ActuatorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _MultiActuatorsBase : public _ModuleBase 17 | { 18 | public: 19 | _MultiActuatorsBase(); 20 | ~_MultiActuatorsBase(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int link(void); 24 | virtual int start(void); 25 | virtual int check(void); 26 | 27 | virtual void setPtargetAll(float nP); 28 | virtual void setStargetAll(float nS); 29 | virtual void gotoOriginAll(void); 30 | virtual bool bCompleteAll(void); 31 | 32 | protected: 33 | virtual void updateActuators(void); 34 | virtual void update(void); 35 | static void *getUpdate(void *This) 36 | { 37 | ((_MultiActuatorsBase *)This)->update(); 38 | return NULL; 39 | } 40 | 41 | protected: 42 | vector<_ActuatorBase*> m_vAB; 43 | }; 44 | 45 | } 46 | #endif 47 | -------------------------------------------------------------------------------- /src/Arithmetic/Destimator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Destimator.cpp 3 | * 4 | * Created on: Mar 22, 2021 5 | * Author: yankai 6 | */ 7 | 8 | #include "Destimator.h" 9 | 10 | namespace kai 11 | { 12 | 13 | Destimator::Destimator() 14 | { 15 | } 16 | 17 | Destimator::~Destimator() 18 | { 19 | } 20 | 21 | int Destimator::init(void *pKiss) 22 | { 23 | CHECK_(this->BASE::init(pKiss)); 24 | Kiss *pK = (Kiss *)pKiss; 25 | 26 | return OK_OK; 27 | } 28 | 29 | double Destimator::v(int x) 30 | { 31 | return 0.0; // m_vOut; 32 | } 33 | 34 | } 35 | -------------------------------------------------------------------------------- /src/Arithmetic/Destimator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Destimator.h 3 | * 4 | * Created on: Mar 22, 2021 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Arithmetic_Destimator_H_ 9 | #define OpenKAI_src_Arithmetic_Destimator_H_ 10 | 11 | #include "../Base/BASE.h" 12 | #include "../Module/Kiss.h" 13 | #include "PolyFit.h" 14 | 15 | namespace kai 16 | { 17 | 18 | class Destimator : public BASE 19 | { 20 | public: 21 | Destimator(); 22 | virtual ~Destimator(); 23 | 24 | virtual int init(void* pKiss); 25 | virtual double v(int x); 26 | 27 | protected: 28 | PolyFit m_fit; 29 | 30 | }; 31 | 32 | } 33 | #endif 34 | -------------------------------------------------------------------------------- /src/Arithmetic/PolyFit.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PolyFit.h 3 | * 4 | * Created on: Sept 12, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Arithmetic_PolyFit_H_ 9 | #define OpenKAI_src_Arithmetic_PolyFit_H_ 10 | 11 | #include "../Base/common.h" 12 | #include 13 | 14 | namespace kai 15 | { 16 | 17 | class PolyFit 18 | { 19 | public: 20 | PolyFit(); 21 | virtual ~PolyFit(); 22 | 23 | virtual bool init(int n, int nDeg); 24 | virtual void x(int i, double y); 25 | virtual void fit(void); 26 | virtual double yPoly(int x); 27 | 28 | protected: 29 | gsl_multifit_linear_workspace *m_gWS; 30 | gsl_matrix *m_gCov, *m_gX; 31 | gsl_vector *m_gY, *m_gC; 32 | 33 | int m_n; 34 | int m_nDeg; 35 | double *m_pY; 36 | double *m_pPoly; 37 | }; 38 | 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_GPS.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_GPS_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_GPS_H_ 3 | 4 | #include "../../Filter/Median.h" 5 | #include "../../Navigation/Coordinate.h" 6 | #include "../../Navigation/_NavBase.h" 7 | #include "_APmavlink_base.h" 8 | 9 | namespace kai 10 | { 11 | 12 | class _APmavlink_GPS : public _ModuleBase 13 | { 14 | public: 15 | _APmavlink_GPS(); 16 | ~_APmavlink_GPS(); 17 | 18 | int init(void *pKiss); 19 | int start(void); 20 | int check(void); 21 | void update(void); 22 | void console(void *pConsole); 23 | 24 | bool reset(void); 25 | 26 | protected: 27 | void updateGPS(void); 28 | static void *getUpdate(void *This) 29 | { 30 | ((_APmavlink_GPS *)This)->update(); 31 | return NULL; 32 | } 33 | 34 | public: 35 | _APmavlink_base *m_pAP; 36 | _NavBase *m_pSB; 37 | Coordinate m_GPS; 38 | 39 | double m_yaw; 40 | bool m_bYaw; 41 | int m_iRelayLED; 42 | 43 | vInt3 m_vAxisIdx; 44 | vFloat3 m_vAxisK; 45 | 46 | LL_POS m_llPos; 47 | UTM_POS m_utmPos; 48 | LL_POS m_llOrigin; 49 | UTM_POS m_utmOrigin; 50 | bool m_bUseApOrigin; 51 | 52 | mavlink_gps_input_t m_D; 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_avoid.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_avoid_H_ 3 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_avoid_H_ 4 | 5 | #include "../../Detector/_DetectorBase.h" 6 | #include "_APmavlink_base.h" 7 | 8 | namespace kai 9 | { 10 | 11 | class _APmavlink_avoid : public _ModuleBase 12 | { 13 | public: 14 | _APmavlink_avoid(); 15 | ~_APmavlink_avoid(); 16 | 17 | int init(void *pKiss); 18 | int start(void); 19 | int check(void); 20 | void update(void); 21 | void draw(void *pFrame); 22 | void console(void *pConsole); 23 | 24 | private: 25 | void updateTarget(void); 26 | static void *getUpdate(void *This) 27 | { 28 | ((_APmavlink_avoid *)This)->update(); 29 | return NULL; 30 | } 31 | 32 | private: 33 | _APmavlink_base *m_pAP; 34 | _DetectorBase *m_pDet; 35 | _Mavlink *m_pMavlink; 36 | 37 | _Object m_obs; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_depthVision.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef OpenKAI_src_Autopilot__APmavlink_depthVision_H_ 3 | #define OpenKAI_src_Autopilot__APmavlink_depthVision_H_ 4 | 5 | #include "../../Vision/RGBD/_RGBDbase.h" 6 | #include "_APmavlink_base.h" 7 | 8 | #define N_DEPTH_ROI 16 9 | 10 | namespace kai 11 | { 12 | 13 | struct DEPTH_ROI 14 | { 15 | uint8_t m_orientation; 16 | vFloat4 m_roi; 17 | float m_minD; 18 | 19 | void init(void) 20 | { 21 | m_minD = 0.0; 22 | m_orientation = 0; 23 | m_roi.clear(); 24 | } 25 | }; 26 | 27 | class _APmavlink_depthVision : public _ModuleBase 28 | { 29 | public: 30 | _APmavlink_depthVision(); 31 | ~_APmavlink_depthVision(); 32 | 33 | int init(void *pKiss); 34 | void update(void); 35 | void draw(void *pFrame); 36 | 37 | private: 38 | _APmavlink_base *m_pAP; 39 | _RGBDbase *m_pDV; 40 | 41 | int m_nROI; 42 | DEPTH_ROI m_pROI[N_DEPTH_ROI]; 43 | }; 44 | 45 | } 46 | #endif 47 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_distLidar.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_distLidar_H_ 3 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_distLidar_H_ 4 | 5 | #include "../../Sensor/Distance/_DistSensorBase.h" 6 | #include "_APmavlink_base.h" 7 | 8 | #define N_LIDAR_SECTION 10 9 | 10 | namespace kai 11 | { 12 | 13 | struct DIST_LIDAR_SECTION 14 | { 15 | uint8_t m_orientation; 16 | double m_degFrom; 17 | double m_degTo; 18 | double m_minD; 19 | double m_sensorScale; 20 | 21 | void init(void) 22 | { 23 | m_orientation = 0; 24 | m_degFrom = 360 - 22.5; 25 | m_degTo = 360 + 22.5; 26 | m_minD = 0.0; 27 | m_sensorScale = 1.0; 28 | } 29 | }; 30 | 31 | class _APmavlink_distLidar : public _ModuleBase 32 | { 33 | public: 34 | _APmavlink_distLidar(); 35 | ~_APmavlink_distLidar(); 36 | 37 | int init(void *pKiss); 38 | void update(void); 39 | void draw(void *pFrame); 40 | 41 | private: 42 | void updateMavlink(void); 43 | 44 | _APmavlink_base *m_pAP; 45 | _DistSensorBase *m_pDS; 46 | 47 | int m_nSection; 48 | DIST_LIDAR_SECTION m_pSection[N_LIDAR_SECTION]; 49 | }; 50 | 51 | } 52 | #endif 53 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_drive.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_drive_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_drive_H_ 3 | 4 | #include "_APmavlink_base.h" 5 | #include "../Drive/_Drive.h" 6 | 7 | namespace kai 8 | { 9 | 10 | class _APmavlink_drive : public _ModuleBase 11 | { 12 | public: 13 | _APmavlink_drive(); 14 | ~_APmavlink_drive(); 15 | 16 | virtual int init(void *pKiss); 17 | virtual int start(void); 18 | virtual int check(void); 19 | virtual void update(void); 20 | virtual void console(void *pConsole); 21 | 22 | virtual void setYawMode(bool bRelative); 23 | 24 | protected: 25 | virtual void onPause(void); 26 | 27 | bool updateDrive(void); 28 | static void *getUpdate(void *This) 29 | { 30 | ((_APmavlink_drive *)This)->update(); 31 | return NULL; 32 | } 33 | 34 | public: 35 | _APmavlink_base *m_pAP; 36 | _Drive *m_pD; 37 | 38 | bool m_bSetYawSpeed; 39 | float m_yawMode; 40 | bool m_bRcChanOverride; 41 | float m_pwmM; 42 | float m_pwmD; 43 | uint16_t *m_pRcYaw; 44 | uint16_t *m_pRcThrottle; 45 | mavlink_rc_channels_override_t m_rcOverride; 46 | }; 47 | 48 | } 49 | #endif 50 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_httpJson.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_httpJson_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_httpJson_H_ 3 | 4 | #include "../../Module/JSON.h" 5 | #include "../../Net/HttpClient.h" 6 | #include "_APmavlink_base.h" 7 | 8 | namespace kai 9 | { 10 | 11 | class _APmavlink_httpJson : public _ModuleBase 12 | { 13 | public: 14 | _APmavlink_httpJson(); 15 | ~_APmavlink_httpJson(); 16 | 17 | virtual int init(void *pKiss); 18 | virtual int link(void); 19 | virtual int start(void); 20 | virtual void update(void); 21 | virtual void console(void *pConsole); 22 | virtual int check(void); 23 | 24 | private: 25 | void updateHttpSend(void); 26 | static void *getUpdate(void *This) 27 | { 28 | ((_APmavlink_httpJson *)This)->update(); 29 | return NULL; 30 | } 31 | 32 | protected: 33 | _APmavlink_base *m_pAP; 34 | 35 | HttpClient m_httpC; 36 | string m_url; 37 | }; 38 | 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_mav2json.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_mav2json_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_mav2json_H_ 3 | 4 | #include "../../Protocol/_JSONbase.h" 5 | #include "_APmavlink_base.h" 6 | 7 | namespace kai 8 | { 9 | class _APmavlink_mav2json : public _JSONbase 10 | { 11 | public: 12 | _APmavlink_mav2json(); 13 | ~_APmavlink_mav2json(); 14 | 15 | virtual int init(void *pKiss); 16 | virtual int link(void); 17 | virtual int start(void); 18 | virtual int check(void); 19 | virtual void console(void *pConsole); 20 | 21 | protected: 22 | virtual void send(void); 23 | virtual void handleJson(const string &str); 24 | 25 | virtual void handleHeartbeat(picojson::object &o); 26 | virtual void handleStat(picojson::object &o); 27 | 28 | private: 29 | void updateW(void); 30 | static void *getUpdateW(void *This) 31 | { 32 | ((_APmavlink_mav2json *)This)->updateW(); 33 | return NULL; 34 | } 35 | 36 | void updateR(void); 37 | static void *getUpdateR(void *This) 38 | { 39 | ((_APmavlink_mav2json *)This)->updateR(); 40 | return NULL; 41 | } 42 | 43 | protected: 44 | _APmavlink_base *m_pAP; 45 | }; 46 | 47 | } 48 | #endif 49 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_photo.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_photo_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_photo_H_ 3 | 4 | #include "../../Vision/RGBD/_RGBDbase.h" 5 | #include "../../Vision/_GPhoto.h" 6 | #include "../../Sensor/Distance/_DistSensorBase.h" 7 | #include "_APmavlink_base.h" 8 | #include "_APmavlink_move.h" 9 | 10 | namespace kai 11 | { 12 | 13 | class _APmavlink_photo : public _ModuleBase 14 | { 15 | public: 16 | _APmavlink_photo(); 17 | ~_APmavlink_photo(); 18 | 19 | int init(void *pKiss); 20 | int link(void); 21 | int start(void); 22 | void update(void); 23 | int check(void); 24 | void console(void *pConsole); 25 | 26 | private: 27 | void updatePhoto(void); 28 | static void *getUpdate(void *This) 29 | { 30 | ((_APmavlink_photo *)This)->update(); 31 | return NULL; 32 | } 33 | 34 | protected: 35 | _APmavlink_base *m_pAP; 36 | int m_iTake; 37 | string m_dir; 38 | string m_subDir; 39 | string m_exifConfig; 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_rcChannel.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_rcChannel_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_rcChannel_H_ 3 | 4 | #include "_APmavlink_base.h" 5 | #include "../../Utility/RC.h" 6 | 7 | namespace kai 8 | { 9 | 10 | class _APmavlink_rcChannel : public _ModuleBase 11 | { 12 | public: 13 | _APmavlink_rcChannel(); 14 | ~_APmavlink_rcChannel(); 15 | 16 | int init(void *pKiss); 17 | int link(void); 18 | int start(void); 19 | int check(void); 20 | void update(void); 21 | void console(void *pConsole); 22 | 23 | private: 24 | void updateRCchannel(void); 25 | static void *getUpdate(void *This) 26 | { 27 | ((_APmavlink_rcChannel *)This)->update(); 28 | return NULL; 29 | } 30 | 31 | private: 32 | _APmavlink_base *m_pAP; 33 | RC_CHANNEL m_rcMode; 34 | RC_CHANNEL m_rcStickV; 35 | RC_CHANNEL m_rcStickH; 36 | }; 37 | 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_relay.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_relay_H_ 3 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_relay_H_ 4 | 5 | #include "_APmavlink_base.h" 6 | 7 | namespace kai 8 | { 9 | 10 | struct AP_relay 11 | { 12 | uint8_t m_iChan; 13 | bool m_bRelay; 14 | 15 | void init(void) 16 | { 17 | m_iChan = 9; 18 | m_bRelay = false; 19 | } 20 | }; 21 | 22 | class _APmavlink_relay : public _ModuleBase 23 | { 24 | public: 25 | _APmavlink_relay(); 26 | ~_APmavlink_relay(); 27 | 28 | int init(void *pKiss); 29 | int start(void); 30 | int check(void); 31 | void update(void); 32 | void console(void *pConsole); 33 | 34 | private: 35 | void updateRelay(void); 36 | static void *getUpdate(void *This) 37 | { 38 | ((_APmavlink_relay *)This)->update(); 39 | return NULL; 40 | } 41 | 42 | private: 43 | _APmavlink_base *m_pAP; 44 | vector m_vRelay; 45 | }; 46 | 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_servo.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_servo_H_ 3 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_servo_H_ 4 | 5 | #include "_APmavlink_base.h" 6 | 7 | namespace kai 8 | { 9 | 10 | struct AP_SERVO 11 | { 12 | uint8_t m_iChan; 13 | uint16_t m_pwm; 14 | 15 | void init(void) 16 | { 17 | m_iChan = 9; 18 | m_pwm = 1500; 19 | } 20 | }; 21 | 22 | class _APmavlink_servo : public _ModuleBase 23 | { 24 | public: 25 | _APmavlink_servo(); 26 | ~_APmavlink_servo(); 27 | 28 | int init(void *pKiss); 29 | int start(void); 30 | int check(void); 31 | void update(void); 32 | void console(void *pConsole); 33 | 34 | private: 35 | void updateServo(void); 36 | static void *getUpdate(void *This) 37 | { 38 | ((_APmavlink_servo *)This)->update(); 39 | return NULL; 40 | } 41 | 42 | private: 43 | _APmavlink_base *m_pAP; 44 | vector m_vServo; 45 | }; 46 | 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_video.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_video_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_video_H_ 3 | 4 | #include "../../IO/_File.h" 5 | #include "../../Net/_Uploader.h" 6 | #include "_APmavlink_base.h" 7 | #include "../../Utility/utilCV.h" 8 | 9 | using namespace picojson; 10 | 11 | namespace kai 12 | { 13 | 14 | class _APmavlink_video : public _ModuleBase 15 | { 16 | public: 17 | _APmavlink_video(); 18 | ~_APmavlink_video(); 19 | 20 | int init(void *pKiss); 21 | int link(void); 22 | int start(void); 23 | void update(void); 24 | int check(void); 25 | void console(void *pConsole); 26 | 27 | private: 28 | void onResume(void); 29 | void onPause(void); 30 | 31 | bool openStream(void); 32 | void closeStream(void); 33 | void writeStream(void); 34 | static void *getUpdate(void *This) 35 | { 36 | ((_APmavlink_video *)This)->update(); 37 | return NULL; 38 | } 39 | 40 | private: 41 | _APmavlink_base *m_pAP; 42 | _Uploader* m_pCurl; 43 | string m_fName; 44 | 45 | string m_process; 46 | FILE* m_pFvid; 47 | uint64_t m_tRecStart; 48 | 49 | bool m_bMeta; 50 | _File* m_pFmeta; 51 | string m_fCalib; 52 | Mat m_mC; 53 | Mat m_mD; 54 | string m_dir; 55 | }; 56 | 57 | } 58 | #endif 59 | -------------------------------------------------------------------------------- /src/Autopilot/APmavlink/_APmavlink_videoStream.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Autopilot_APmavlink__APmavlink_videoStream_H_ 2 | #define OpenKAI_src_Autopilot_APmavlink__APmavlink_videoStream_H_ 3 | 4 | #include "../../IO/_File.h" 5 | #include "../../Net/_Uploader.h" 6 | #include "_APmavlink_base.h" 7 | #include "../../Utility/util.h" 8 | 9 | using namespace picojson; 10 | 11 | namespace kai 12 | { 13 | 14 | class _APmavlink_videoStream : public _ModuleBase 15 | { 16 | public: 17 | _APmavlink_videoStream(); 18 | ~_APmavlink_videoStream(); 19 | 20 | int init(void *pKiss); 21 | int link(void); 22 | int start(void); 23 | void update(void); 24 | int check(void); 25 | void console(void *pConsole); 26 | 27 | private: 28 | void updateStream(void); 29 | bool openStream(void); 30 | void closeStream(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_APmavlink_videoStream *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | private: 38 | _APmavlink_base *m_pAP; 39 | _Uploader* m_pCurl; 40 | string m_fName; 41 | string m_process; 42 | string m_dir; 43 | 44 | int m_iWP; 45 | vector m_vWP; 46 | 47 | int m_gstPID; 48 | uint64_t m_tVidInt; 49 | uint64_t m_tRecStart; 50 | }; 51 | 52 | } 53 | #endif 54 | -------------------------------------------------------------------------------- /src/Base/BASE.h: -------------------------------------------------------------------------------- 1 | /* 2 | * BASE.h 3 | * 4 | * Created on: Sep 15, 2016 5 | * Author: Kai Yan 6 | */ 7 | 8 | #ifndef OpenKAI_src_Base_BASE_H_ 9 | #define OpenKAI_src_Base_BASE_H_ 10 | 11 | #include "common.h" 12 | 13 | using namespace std; 14 | 15 | namespace kai 16 | { 17 | class BASE 18 | { 19 | public: 20 | BASE(); 21 | virtual ~BASE(); 22 | 23 | virtual int init(void *pKiss); 24 | virtual int link(void); 25 | virtual int start(void); 26 | virtual int check(void); 27 | 28 | virtual void pause(void); 29 | virtual void resume(void); 30 | virtual void stop(void); 31 | 32 | virtual void draw(void *pFrame); 33 | virtual void console(void *pConsole); 34 | virtual void context(void *pContext); 35 | 36 | virtual int serialize(uint8_t *pB, int nB); 37 | virtual int deSerialize(uint8_t *pB, int nB); 38 | 39 | string getName(void); 40 | string getClass(void); 41 | 42 | protected: 43 | void *m_pKiss; 44 | bool m_bLog; 45 | }; 46 | 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/Base/common.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Base_common_H_ 2 | #define OpenKAI_src_Base_common_H_ 3 | 4 | #ifdef USE_OPEN3D 5 | #include "open3d.h" 6 | #endif 7 | 8 | #ifdef USE_OPENCV 9 | #include "cv.h" 10 | #endif 11 | 12 | #ifdef USE_MATHGL 13 | #include 14 | #include 15 | #endif 16 | 17 | #ifdef USE_GLOG 18 | #include 19 | #endif 20 | 21 | #include 22 | 23 | #include "platform.h" 24 | #include "macro.h" 25 | #include "constant.h" 26 | #include "../Primitive/vInt2.h" 27 | #include "../Primitive/vInt3.h" 28 | #include "../Primitive/vInt4.h" 29 | #include "../Primitive/vFloat2.h" 30 | #include "../Primitive/vFloat3.h" 31 | #include "../Primitive/vFloat4.h" 32 | #include "../Primitive/vDouble2.h" 33 | #include "../Primitive/vDouble3.h" 34 | #include "../Primitive/vDouble4.h" 35 | #include "../Primitive/primitiveUtil.h" 36 | #include "../Primitive/tSwap.h" 37 | #include "../Utility/util.h" 38 | #include "../Utility/utilTime.h" 39 | #include "../Utility/utilEvent.h" 40 | #include "../Utility/utilStr.h" 41 | 42 | #endif 43 | 44 | -------------------------------------------------------------------------------- /src/Base/cv.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #ifdef USE_CUDA 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | using namespace cv::cuda; 21 | #endif 22 | 23 | using namespace cv; 24 | using namespace dnn; 25 | -------------------------------------------------------------------------------- /src/Base/platform.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include "stdio.h" 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | using namespace std; 51 | using std::string; 52 | 53 | -------------------------------------------------------------------------------- /src/Compute/OpenCL/clBase.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Control_clBase_H_ 2 | #define OpenKAI_src_Control_clBase_H_ 3 | 4 | #include "../../Base/BASE.h" 5 | #include "../../Module/Kiss.h" 6 | 7 | #define CL_TARGET_OPENCL_VERSION 300 8 | #include 9 | 10 | #define CLBASE_SRC_NB 102400 11 | 12 | namespace kai 13 | { 14 | 15 | class clBase : public BASE 16 | { 17 | public: 18 | clBase(); 19 | virtual ~clBase(); 20 | 21 | virtual int init(void *pKiss); 22 | virtual int link(void); 23 | virtual int check(void); 24 | 25 | protected: 26 | virtual void getPlatformInfo(void); 27 | 28 | virtual bool setupCL(void); 29 | virtual bool setupKernel(void); 30 | 31 | protected: 32 | int m_iTargetPlatformIdx; 33 | int m_iTargetDevIdx; 34 | 35 | string m_targetPlatformName; 36 | string m_targetDevName; 37 | 38 | cl_device_id m_clDev; 39 | 40 | cl_context m_clContext; 41 | cl_context_properties* m_pCLcontextProps; 42 | cl_queue_properties* m_pCLqProp; 43 | 44 | cl_command_queue m_clCmdQ; 45 | 46 | string m_fKernel; 47 | string m_buildOpt; 48 | string m_kName; 49 | cl_program m_clProgram; 50 | cl_kernel m_clKernel; 51 | }; 52 | 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /src/Control/PID.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PIDctrl.h 3 | * 4 | * Created on: May 18, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Control_PID_H_ 9 | #define OpenKAI_src_Control_PID_H_ 10 | 11 | #include "../Base/BASE.h" 12 | #include "../Module/Kiss.h" 13 | #include "../UI/_Console.h" 14 | 15 | namespace kai 16 | { 17 | 18 | class PID : public BASE 19 | { 20 | public: 21 | PID(); 22 | virtual ~PID(); 23 | 24 | virtual int init(void *pKiss); 25 | virtual float update(float v, float sp, float dT); 26 | virtual double o(void); 27 | virtual void console(void *pConsole); 28 | virtual void reset(void); 29 | 30 | protected: 31 | float m_P; 32 | float m_I; 33 | float m_Imax; 34 | float m_D; 35 | 36 | float m_vVar; 37 | float m_vSetPoint; 38 | float m_e; 39 | float m_eOld; 40 | float m_eI; //e integration 41 | 42 | float m_vOut; 43 | vFloat2 m_vRin; 44 | vFloat2 m_vRout; 45 | }; 46 | 47 | } 48 | #endif 49 | -------------------------------------------------------------------------------- /src/DNN/JetsonInference/_DetectNet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DetectNet.h 3 | * 4 | * Created on: Aug 17, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_DNN_JetsonInference__DetectNet_H_ 9 | #define OpenKAI_src_DNN_JetsonInference__DetectNet_H_ 10 | 11 | #include "detectNet.h" 12 | #include "../../Vision/_VisionBase.h" 13 | #include "../../Detector/_DetectorBase.h" 14 | 15 | namespace kai 16 | { 17 | 18 | enum DETECTNET_TYPE 19 | { 20 | detectNet_uff, 21 | detectNet_caffe, 22 | }; 23 | 24 | class _DetectNet: public _DetectorBase 25 | { 26 | public: 27 | _DetectNet(); 28 | ~_DetectNet(); 29 | 30 | int init(void* pKiss); 31 | int start(void); 32 | int check(void); 33 | 34 | private: 35 | bool open(void); 36 | void detect(void); 37 | void update(void); 38 | static void* getUpdate(void* This) 39 | { 40 | ((_DetectNet*) This)->update(); 41 | return NULL; 42 | } 43 | 44 | public: 45 | detectNet* m_pDN; 46 | int m_nBox; 47 | uint32_t m_nClass; 48 | DETECTNET_TYPE m_type; 49 | float m_thr; 50 | string m_layerIn; 51 | string m_layerOut; 52 | string m_layerNboxOut; 53 | vInt3 m_vDimIn; 54 | int m_nMaxBatch; 55 | int m_precision; 56 | int m_device; 57 | bool m_bAllowGPUfallback; 58 | 59 | Frame* m_pRGBA; 60 | Frame* m_pRGBAf; 61 | bool m_bSwapRB; 62 | vFloat3 m_vMean; 63 | 64 | }; 65 | 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /src/DNN/TensorFlowLite/_TFmobileNet.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _TFmobileNet.h 3 | * 4 | * Created on: July 15, 2023 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_DNN_TensorFlowLite__TFmobileNet_H_ 9 | #define OpenKAI_src_DNN_TensorFlowLite__TFmobileNet_H_ 10 | 11 | #include "../../Vision/_VisionBase.h" 12 | #include "../../Detector/_DetectorBase.h" 13 | 14 | #include "tensorflow/lite/interpreter.h" 15 | #include "tensorflow/lite/kernels/register.h" 16 | #include "tensorflow/lite/string_util.h" 17 | #include "tensorflow/lite/model.h" 18 | 19 | namespace kai 20 | { 21 | 22 | class _TFmobileNet : public _DetectorBase 23 | { 24 | public: 25 | _TFmobileNet(); 26 | ~_TFmobileNet(); 27 | 28 | int init(void *pKiss); 29 | int start(void); 30 | int check(void); 31 | 32 | private: 33 | bool open(void); 34 | void detect(void); 35 | void update(void); 36 | static void *getUpdate(void *This) 37 | { 38 | ((_TFmobileNet *)This)->update(); 39 | return NULL; 40 | } 41 | 42 | public: 43 | vInt2 m_vSize; 44 | std::vector m_Labels; 45 | std::unique_ptr m_interpreter; 46 | std::unique_ptr m_model; 47 | 48 | int m_nThreads; 49 | float m_confidence; 50 | }; 51 | 52 | } 53 | #endif 54 | -------------------------------------------------------------------------------- /src/Data/Image/_BBoxCutOut.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _BBoxCutOut.h 3 | * 4 | * Created on: Sept 16, 2017 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_File_Image__BBoxCutOut_H_ 9 | #define OpenKAI_src_File_Image__BBoxCutOut_H_ 10 | 11 | #include "../../Base/_ModuleBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _BBoxCutOut : public _ModuleBase 17 | { 18 | public: 19 | _BBoxCutOut(); 20 | ~_BBoxCutOut(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | 25 | private: 26 | void process(); 27 | void update(void); 28 | static void *getUpdate(void *This) 29 | { 30 | ((_BBoxCutOut *)This)->update(); 31 | return NULL; 32 | } 33 | 34 | public: 35 | string m_dirIn; 36 | string m_dirOut; 37 | string m_extTxt; 38 | string m_extImgIn; 39 | string m_extImgOut; 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/Data/Image/_GDimgUploader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _GDimgUploader.h 3 | * 4 | * Created on: Feb 26, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Application__GDimgUploader_H_ 9 | #define OpenKAI_src_Application__GDimgUploader_H_ 10 | 11 | #include "../../Vision/_VisionBase.h" 12 | #include "../../Module/JSON.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _GDimgUploader : public _ModuleBase 18 | { 19 | public: 20 | _GDimgUploader(void); 21 | virtual ~_GDimgUploader(); 22 | 23 | int init(void *pKiss); 24 | int start(void); 25 | int check(void); 26 | 27 | private: 28 | void updateUpload(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_GDimgUploader *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | private: 37 | _VisionBase *m_pV; 38 | 39 | uint64_t m_tInterval; 40 | uint64_t m_tLastUpload; 41 | vector m_vJPGquality; 42 | string m_tempDir; 43 | fstream m_fMeta; 44 | string m_gdUpload; 45 | string m_gdFolderID; 46 | string m_gdCredentials; 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /src/Data/Image/_GPhotoTake.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_File__GPhotoTake_H_ 2 | #define OpenKAI_src_File__GPhotoTake_H_ 3 | 4 | #include "../../Vision/_GPhoto.h" 5 | #include "../_FileBase.h" 6 | 7 | namespace kai 8 | { 9 | 10 | class _GPhotoTake : public _FileBase 11 | { 12 | public: 13 | _GPhotoTake(); 14 | ~_GPhotoTake(); 15 | 16 | virtual int init(void *pKiss); 17 | virtual int link(void); 18 | virtual int start(void); 19 | virtual int check(void); 20 | virtual void update(void); 21 | virtual void console(void *pConsole); 22 | 23 | bool shutter(void); 24 | bool startAutoMode(int nTake = 0, int tInterval = 1000); 25 | void stopAutoMode(void); 26 | 27 | private: 28 | void updateTake(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_GPhotoTake *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | private: 36 | _GPhoto *m_pG; 37 | 38 | int m_iTake; 39 | uint64_t m_tDelay; 40 | 41 | string m_dir; 42 | string m_subDir; 43 | bool m_bFlipRGB; 44 | bool m_bFlipD; 45 | vector m_compress; 46 | int m_quality; 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /src/Data/_FileBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _FileBase.h 3 | * 4 | * Created on: Oct 16, 2017 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_File__FileBase_H_ 9 | #define OpenKAI_src_File__FileBase_H_ 10 | 11 | #include "../Base/_ModuleBase.h" 12 | 13 | #define D_TYPE_FILE 0x8 14 | #define D_TYPE_FOLDER 0x4 15 | 16 | namespace kai 17 | { 18 | 19 | class _FileBase : public _ModuleBase 20 | { 21 | public: 22 | _FileBase(); 23 | ~_FileBase(); 24 | 25 | virtual int init(void *pKiss); 26 | virtual int start(void); 27 | 28 | bool createDir(const string &dir); 29 | bool removeDir(const string &dir); 30 | string getFirstSubDir(const string &baseDir); 31 | bool getDirFileList(const string &dir, vector* pvFile, vector* pvExt = NULL); 32 | string getExtension(const string &fName); 33 | bool bExtension(const string &fName, const vector& vExt); 34 | 35 | private: 36 | void update(void); 37 | static void *getUpdate(void *This) 38 | { 39 | ((_FileBase *)This)->update(); 40 | return NULL; 41 | } 42 | 43 | public: 44 | vector m_vExt; 45 | }; 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /src/Dependencies/Feetech/INST.h: -------------------------------------------------------------------------------- 1 | /* 2 | * INST.h 3 | * 飞特串行舵机协议指令定义 4 | * 日期: 2021.12.8 5 | * 作者: 6 | */ 7 | 8 | #ifndef _INST_H 9 | #define _INST_H 10 | 11 | typedef char s8; 12 | typedef unsigned char u8; 13 | typedef unsigned short u16; 14 | typedef short s16; 15 | 16 | typedef unsigned long u32; 17 | typedef long s32; 18 | 19 | #define INST_PING 0x01 20 | #define INST_READ 0x02 21 | #define INST_WRITE 0x03 22 | #define INST_REG_WRITE 0x04 23 | #define INST_REG_ACTION 0x05 24 | #define INST_SYNC_READ 0x82 25 | #define INST_SYNC_WRITE 0x83 26 | 27 | #endif -------------------------------------------------------------------------------- /src/Dependencies/Feetech/SCSerial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SCSerial.h 3 | * 飞特串行舵机硬件接口层程序 4 | * 日期: 2022.3.29 5 | * 作者: 6 | */ 7 | 8 | #ifndef _SCSERIAL_H 9 | #define _SCSERIAL_H 10 | 11 | #include "SCS.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | class SCSerial : public SCS 20 | { 21 | public: 22 | SCSerial(); 23 | SCSerial(u8 End); 24 | SCSerial(u8 End, u8 Level); 25 | 26 | protected: 27 | int writeSCS(unsigned char *nDat, int nLen);//输出nLen字节 28 | int readSCS(unsigned char *nDat, int nLen);//输入nLen字节 29 | int writeSCS(unsigned char bDat);//输出1字节 30 | void rFlushSCS();// 31 | void wFlushSCS();// 32 | public: 33 | unsigned long int IOTimeOut;//输入输出超时 34 | int Err; 35 | public: 36 | virtual int getErr(){ return Err; } 37 | virtual int setBaudRate(int baudRate); 38 | virtual bool begin(int baudRate, const char* serialPort); 39 | virtual void end(); 40 | protected: 41 | int fd;//serial port handle 42 | struct termios orgopt;//fd ort opt 43 | struct termios curopt;//fd cur opt 44 | unsigned char txBuf[255]; 45 | int txBufLen; 46 | }; 47 | 48 | #endif -------------------------------------------------------------------------------- /src/Dependencies/Feetech/SCServo.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SCServo.h 3 | * 飞特串行舵机接口 4 | * 日期: 2021.12.8 5 | * 作者: 6 | */ 7 | 8 | #ifndef _SCSERVO_H 9 | #define _SCSERVO_H 10 | 11 | #include "SMSBL.h" 12 | #include "SCSCL.h" 13 | #include "SMSCL.h" 14 | #include "SMS_STS.h" 15 | #endif -------------------------------------------------------------------------------- /src/Dependencies/Feetech/SMSCL.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/src/Dependencies/Feetech/SMSCL.cpp -------------------------------------------------------------------------------- /src/Dependencies/Feetech/SMSCL.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yankailab/OpenKAI/c514ed582a2a7c8de23f9ed68b17646482bef06a/src/Dependencies/Feetech/SMSCL.h -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/ASLUAV/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ASLUAV.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -5106489462006643972 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "ASLUAV.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/ASLUAV/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ASLUAV.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/AVSSUAS/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from AVSSUAS.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -1591256252692971080 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "AVSSUAS.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/AVSSUAS/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from AVSSUAS.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/all/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from all.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 2586536748364934100 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "all.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/all/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from all.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/ardupilotmega/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 7531975446324603559 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "ardupilotmega.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/ardupilotmega/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ardupilotmega.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/common/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 9100490121178045632 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "common.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/common/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/csAirLink/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from csAirLink.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 5236145736010701632 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "csAirLink.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/csAirLink/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from csAirLink.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 100 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/cubepilot/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from cubepilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 4776168416021005157 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "cubepilot.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/cubepilot/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from cubepilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/development/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from development.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -1668051321899454654 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "development.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/development/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from development.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/icarous/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from icarous.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 2030998271043869307 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "icarous.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/icarous/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from icarous.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 46 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/matrixpilot/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -2937009946209942950 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "matrixpilot.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/matrixpilot/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from matrixpilot.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/message_definitions/standard.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | minimal.xml 5 | 0 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/minimal/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from minimal.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 6123333502576366225 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "minimal.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/minimal/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from minimal.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 22 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/paparazzi/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from paparazzi.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 5541250286774150658 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "paparazzi.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/paparazzi/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from paparazzi.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/python_array_test/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from python_array_test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 2955094917431610395 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "python_array_test.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/python_array_test/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from python_array_test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/standard/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from standard.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -3094957844812525009 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "standard.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/standard/testsuite.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol testsuite generated from standard.xml 3 | * @see https://mavlink.io/en/ 4 | */ 5 | #pragma once 6 | #ifndef STANDARD_TESTSUITE_H 7 | #define STANDARD_TESTSUITE_H 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | #ifndef MAVLINK_TEST_ALL 14 | #define MAVLINK_TEST_ALL 15 | static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); 16 | static void mavlink_test_standard(uint8_t, uint8_t, mavlink_message_t *last_msg); 17 | 18 | static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 19 | { 20 | mavlink_test_minimal(system_id, component_id, last_msg); 21 | mavlink_test_standard(system_id, component_id, last_msg); 22 | } 23 | #endif 24 | 25 | #include "../minimal/testsuite.h" 26 | 27 | 28 | 29 | static void mavlink_test_standard(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) 30 | { 31 | 32 | } 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif // __cplusplus 37 | #endif // STANDARD_TESTSUITE_H 38 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/standard/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from standard.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 22 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/storm32/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from storm32.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -8473615553365675232 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "storm32.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/storm32/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from storm32.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/test/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 3383248276490699437 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "test.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/test/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from test.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/uAvionix/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from uAvionix.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH -8549850330864572789 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "uAvionix.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/uAvionix/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from uAvionix.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/ualberta/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ualberta.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_HASH 8755730009570358876 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 253 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 1 29 | #endif 30 | 31 | #include "version.h" 32 | #include "ualberta.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /src/Dependencies/c_library_v2/ualberta/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from ualberta.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Thu Aug 01 2024" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /src/Detector/_ArUco.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ArUco.h 3 | * 4 | * Created on: June 15, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__ArUco_H_ 9 | #define OpenKAI_src_Detector__ArUco_H_ 10 | 11 | #include "../Detector/_DetectorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _ArUco : public _DetectorBase 17 | { 18 | public: 19 | _ArUco(); 20 | virtual ~_ArUco(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int start(void); 24 | virtual int check(void); 25 | virtual void draw(void *pFrame); 26 | virtual void console(void *pConsole); 27 | 28 | private: 29 | void detect(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_ArUco *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | protected: 38 | cv::Ptr m_pDict; 39 | uint8_t m_dict; 40 | float m_realSize; 41 | 42 | // optional camera matrix 43 | bool m_bPose; 44 | Mat m_mC; // Intrinsic 45 | Mat m_mCscaled; // scaled with input image size 46 | Mat m_mD; // Distortion 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /src/Detector/_Cascade.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Cascade.h 3 | * 4 | * Created on: Aug 17, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__Cascade_H_ 9 | #define OpenKAI_src_Detector__Cascade_H_ 10 | 11 | #include "../Detector/_DetectorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Cascade : public _DetectorBase 17 | { 18 | public: 19 | _Cascade(); 20 | ~_Cascade(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | int check(void); 25 | 26 | private: 27 | void detectCPU(void); 28 | void detectGPU(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_Cascade *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | cv::CascadeClassifier m_CC; 38 | Ptr m_pGCC; 39 | 40 | bool m_bGPU; 41 | double m_scaleFactor; 42 | int m_minNeighbors; 43 | string m_className; 44 | }; 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /src/Detector/_Chilitags.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Chilitags.h 3 | * 4 | * Created on: June 15, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__Chilitags_H_ 9 | #define OpenKAI_src_Detector__Chilitags_H_ 10 | 11 | #include "../Detector/_DetectorBase.h" 12 | 13 | #ifdef USE_OPENCV 14 | #ifdef USE_CHILITAGS 15 | 16 | #include 17 | 18 | using namespace chilitags; 19 | 20 | namespace kai 21 | { 22 | 23 | class _Chilitags : public _DetectorBase 24 | { 25 | public: 26 | _Chilitags(); 27 | virtual ~_Chilitags(); 28 | 29 | int init(void *pKiss); 30 | int start(void); 31 | void draw(void *pFrame); 32 | void console(void *pConsole); 33 | int check(void); 34 | 35 | private: 36 | void detect(void); 37 | void update(void); 38 | static void *getUpdate(void *This) 39 | { 40 | ((_Chilitags *)This)->update(); 41 | return NULL; 42 | } 43 | 44 | protected: 45 | Chilitags m_chilitags; 46 | int m_persistence; 47 | float m_gain; 48 | float m_angleOffset; 49 | }; 50 | 51 | } 52 | #endif 53 | #endif 54 | #endif 55 | -------------------------------------------------------------------------------- /src/Detector/_Contour.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Contour.h 3 | * 4 | * Created on: Jan 18, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__Contour_H_ 9 | #define OpenKAI_src_Detector__Contour_H_ 10 | 11 | #include "_DetectorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Contour : public _DetectorBase 17 | { 18 | public: 19 | _Contour(); 20 | virtual ~_Contour(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int start(void); 24 | virtual int check(void); 25 | virtual void draw(void *pFrame); 26 | 27 | private: 28 | void detect(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_Contour *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | protected: 37 | }; 38 | 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /src/Detector/_DNNclassifier.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DNNclassifier.h 3 | * 4 | * Created on: Jan 13, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_DNN_Darknet__DNNclassifier_H_ 9 | #define OpenKAI_src_DNN_Darknet__DNNclassifier_H_ 10 | 11 | #include "../Detector/_DetectorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _DNNclassifier : public _DetectorBase 17 | { 18 | public: 19 | _DNNclassifier(); 20 | ~_DNNclassifier(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | void draw(void *pFrame); 25 | int check(void); 26 | bool classify(Mat m, _Object *pO, float minConfidence = 0.0); 27 | 28 | private: 29 | void classify(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_DNNclassifier *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | protected: 38 | cv::dnn::Net m_net; 39 | vector m_vROI; 40 | int m_nW; 41 | int m_nH; 42 | bool m_bSwapRB; 43 | float m_scale; 44 | vInt3 m_vMean; 45 | Mat m_blob; 46 | 47 | int m_iBackend; 48 | int m_iTarget; 49 | }; 50 | 51 | } 52 | #endif 53 | -------------------------------------------------------------------------------- /src/Detector/_DNNtext.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DNNtext.h 3 | * 4 | * Created on: Jan 11, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_DNN_Darknet__DNNtext_H_ 9 | #define OpenKAI_src_DNN_Darknet__DNNtext_H_ 10 | 11 | #include "_YOLOv3.h" 12 | #ifdef USE_OCR 13 | #include "OCR.h" 14 | #endif 15 | 16 | namespace kai 17 | { 18 | 19 | class _DNNtext : public _YOLOv3 20 | { 21 | public: 22 | _DNNtext(); 23 | ~_DNNtext(); 24 | 25 | int init(void *pKiss); 26 | int start(void); 27 | void draw(void *pFrame); 28 | int check(void); 29 | void ocr(void); 30 | 31 | public: 32 | Mat getTransform(Mat mImg, vFloat2 *pBox); 33 | void decode(const Mat &mScores, const Mat &mGeometry, float scoreThresh, 34 | std::vector &vDetections, std::vector &vConfidences); 35 | void detect(void); 36 | void update(void); 37 | static void *getUpdate(void *This) 38 | { 39 | ((_DNNtext *)This)->update(); 40 | return NULL; 41 | } 42 | 43 | public: 44 | vDouble3 m_vMean; 45 | bool m_bDetect; 46 | bool m_bOCR; 47 | bool m_bWarp; 48 | 49 | #ifdef USE_OCR 50 | OCR *m_pOCR; 51 | #endif 52 | }; 53 | 54 | } 55 | #endif 56 | -------------------------------------------------------------------------------- /src/Detector/_DepthSegment.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DepthSegment.h 3 | * 4 | * Created on: May 29, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__DepthSegment_H_ 9 | #define OpenKAI_src_Detector__DepthSegment_H_ 10 | 11 | #include "_DetectorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _DepthSegment : public _DetectorBase 17 | { 18 | public: 19 | _DepthSegment(); 20 | virtual ~_DepthSegment(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | int check(void); 25 | void draw(void *pFrame); 26 | 27 | private: 28 | void detect(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_DepthSegment *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | protected: 37 | Mat m_mR; 38 | float m_rL; 39 | float m_rH; 40 | float m_rD; 41 | float m_rArea; 42 | }; 43 | 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /src/Detector/_DetectorBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DetectorBase.h 3 | * 4 | * Created on: Aug 17, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__DetectorBase_H_ 9 | #define OpenKAI_src_Detector__DetectorBase_H_ 10 | 11 | #include "../Universe/_Universe.h" 12 | #include "../Vision/_VisionBase.h" 13 | #include "../Utility/utilCV.h" 14 | 15 | namespace kai 16 | { 17 | 18 | class _DetectorBase : public _ModuleBase 19 | { 20 | public: 21 | _DetectorBase(); 22 | virtual ~_DetectorBase(); 23 | 24 | virtual int init(void *pKiss); 25 | virtual int link(void); 26 | virtual int check(void); 27 | virtual void console(void *pConsole); 28 | 29 | virtual bool loadModel(void); 30 | virtual int getClassIdx(string &className); 31 | virtual string getClassName(int iClass); 32 | virtual _Universe* getU(void); 33 | 34 | protected: 35 | virtual void onPause(void); 36 | 37 | protected: 38 | // input 39 | _VisionBase *m_pV; 40 | 41 | // data 42 | Frame m_fRGB; 43 | _Universe *m_pU; 44 | 45 | // model 46 | string m_fModel; 47 | string m_fWeight; 48 | string m_fMean; 49 | string m_fClass; 50 | vector m_vClass; 51 | }; 52 | 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /src/Detector/_Line.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Line.h 3 | * 4 | * Created on: April 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__Line_H_ 9 | #define OpenKAI_src_Detector__Line_H_ 10 | 11 | #include "_DetectorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Line : public _DetectorBase 17 | { 18 | public: 19 | _Line(); 20 | virtual ~_Line(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int link(void); 24 | virtual int start(void); 25 | virtual int check(void); 26 | virtual void console(void *pConsole); 27 | 28 | private: 29 | void detect(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_Line *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | public: 38 | _VisionBase *m_pV; 39 | float m_wSlide; 40 | float m_minPixLine; 41 | float m_line; 42 | Mat m_mIn; 43 | }; 44 | 45 | } 46 | #endif 47 | -------------------------------------------------------------------------------- /src/Detector/_MotionDetector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _MotionDetector.h 3 | * 4 | * Created on: Aug 4, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__MotionDetector_H_ 9 | #define OpenKAI_src_Detector__MotionDetector_H_ 10 | 11 | #include "_DetectorBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _MotionDetector : public _DetectorBase 17 | { 18 | public: 19 | _MotionDetector(); 20 | virtual ~_MotionDetector(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | void draw(void *pFrame); 25 | int check(void); 26 | 27 | private: 28 | void detect(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_MotionDetector *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _VisionBase *m_pVision; 38 | string m_algorithm; 39 | cv::Ptr m_pBS; 40 | double m_learningRate; 41 | Mat m_mFG; 42 | }; 43 | 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /src/Detector/_SSD.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _SSD.h 3 | * 4 | * Created on: Jan 11, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_DNN_Darknet__SSD_H_ 9 | #define OpenKAI_src_DNN_Darknet__SSD_H_ 10 | 11 | #include "_DetectorBase.h" 12 | 13 | using namespace cv::dnn; 14 | 15 | namespace kai 16 | { 17 | class _SSD : public _DetectorBase 18 | { 19 | public: 20 | _SSD(); 21 | ~_SSD(); 22 | 23 | virtual int init(void *pKiss); 24 | virtual int start(void); 25 | virtual int check(void); 26 | 27 | private: 28 | void detect(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_SSD *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | protected: 37 | cv::dnn::Net m_net; 38 | double m_thr; 39 | double m_nms; 40 | vInt2 m_vBlobSize; 41 | bool m_bSwapRB; 42 | float m_scale; 43 | vInt3 m_vMean; 44 | Mat m_blob; 45 | int m_iClassDraw; 46 | 47 | int m_iBackend; 48 | int m_iTarget; 49 | }; 50 | 51 | } 52 | #endif 53 | -------------------------------------------------------------------------------- /src/Detector/_YOLOv3.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _YOLOv3.h 3 | * 4 | * Created on: Jan 11, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_DNN_Darknet__YOLOv3_H_ 9 | #define OpenKAI_src_DNN_Darknet__YOLOv3_H_ 10 | 11 | #include "_DetectorBase.h" 12 | 13 | using namespace cv::dnn; 14 | 15 | namespace kai 16 | { 17 | class _YOLOv3 : public _DetectorBase 18 | { 19 | public: 20 | _YOLOv3(); 21 | ~_YOLOv3(); 22 | 23 | virtual int init(void *pKiss); 24 | virtual int start(void); 25 | virtual int check(void); 26 | 27 | private: 28 | void detectYolo(void); 29 | void detect(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_YOLOv3 *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | protected: 38 | cv::dnn::Net m_net; 39 | double m_thr; 40 | double m_nms; 41 | vInt2 m_vBlobSize; 42 | bool m_bSwapRB; 43 | float m_scale; 44 | vInt3 m_vMean; 45 | Mat m_blob; 46 | vector m_vLayerName; 47 | int m_iClassDraw; 48 | 49 | int m_iBackend; 50 | int m_iTarget; 51 | }; 52 | 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /src/Detector/_YOLOv8.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _YOLOv8.h 3 | * 4 | * Created on: Feb 9, 2024 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Detector__YOLOv8_H_ 9 | #define OpenKAI_src_Detector__YOLOv8_H_ 10 | 11 | #include "_DetectorBase.h" 12 | 13 | using namespace cv::dnn; 14 | 15 | namespace kai 16 | { 17 | 18 | class _YOLOv8 : public _DetectorBase 19 | { 20 | public: 21 | _YOLOv8(); 22 | ~_YOLOv8(); 23 | 24 | virtual int init(void *pKiss); 25 | virtual int start(void); 26 | virtual int check(void); 27 | virtual void console(void *pConsole); 28 | 29 | virtual bool loadModel(void); 30 | 31 | private: 32 | Mat formatToSquare(const Mat &mSrc); 33 | void detect(void); 34 | void update(void); 35 | static void *getUpdate(void *This) 36 | { 37 | ((_YOLOv8 *)This)->update(); 38 | return NULL; 39 | } 40 | 41 | protected: 42 | cv::dnn::Net m_net; 43 | float m_confidence; 44 | float m_score; 45 | float m_nms; 46 | bool m_bLetterBoxForSquare; 47 | vInt2 m_vModelInputSize; 48 | bool m_bSwapRB; 49 | float m_scale; 50 | 51 | int m_iBackend; 52 | int m_iTarget; 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /src/Filter/Average.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Average.h 3 | * 4 | * Created on: Nov 18, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Filter_Average_H_ 9 | #define OpenKAI_src_Filter_Average_H_ 10 | 11 | #include "FilterBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | template 17 | class Average : public FilterBase 18 | { 19 | public: 20 | Average() 21 | { 22 | FilterBase::m_nW = 2; 23 | } 24 | virtual ~Average() 25 | { 26 | } 27 | 28 | bool init(int nW) 29 | { 30 | if(nW < 2) 31 | nW = 2; 32 | 33 | FilterBase::m_nW = nW; 34 | FilterBase::reset(); 35 | return true; 36 | } 37 | 38 | T update(T v) 39 | { 40 | FilterBase::add(v); 41 | T tot = 0.0; 42 | int n = FilterBase::m_qV.size(); 43 | for (int i = 0; i < n; i++) 44 | tot += FilterBase::m_qV.at(i); 45 | 46 | FilterBase::m_v = tot / (T)n; 47 | return FilterBase::m_v; 48 | } 49 | }; 50 | 51 | } 52 | #endif 53 | -------------------------------------------------------------------------------- /src/Filter/FilterBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FilterBase.h 3 | * 4 | * Created on: Nov 18, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Filter_FilterBase_H_ 9 | #define OpenKAI_src_Filter_FilterBase_H_ 10 | 11 | #include "../Base/common.h" 12 | 13 | namespace kai 14 | { 15 | template 16 | class FilterBase 17 | { 18 | public: 19 | FilterBase() 20 | { 21 | } 22 | 23 | virtual ~FilterBase() 24 | { 25 | } 26 | 27 | virtual T update(T v) 28 | { 29 | } 30 | 31 | virtual void add(T v) 32 | { 33 | m_qV.push_back(v); 34 | while (m_qV.size() > m_nW) 35 | m_qV.pop_front(); 36 | } 37 | 38 | virtual void reset(void) 39 | { 40 | m_v = 0; 41 | m_qV.clear(); 42 | m_variance = 0; 43 | } 44 | 45 | virtual T get(void) 46 | { 47 | return m_v; 48 | } 49 | 50 | protected: 51 | T m_v; //output 52 | std::deque m_qV; //trajectory 53 | int m_nW; //window length 54 | T m_variance; 55 | }; 56 | 57 | } 58 | #endif 59 | -------------------------------------------------------------------------------- /src/Filter/Median.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Median.h 3 | * 4 | * Created on: Nov 18, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Filter_Median_H_ 9 | #define OpenKAI_src_Filter_Median_H_ 10 | 11 | #include "FilterBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | template 17 | class Median : public FilterBase 18 | { 19 | public: 20 | Median() 21 | { 22 | FilterBase::m_nW = 3; 23 | m_iMid = 1; 24 | } 25 | virtual ~Median() 26 | { 27 | } 28 | 29 | bool init(int nW) 30 | { 31 | if(nW < 3) 32 | nW = 3; 33 | 34 | FilterBase::m_nW = nW; 35 | m_iMid = FilterBase::m_nW / 2; 36 | FilterBase::reset(); 37 | return true; 38 | } 39 | 40 | T update(T v) 41 | { 42 | FilterBase::add(v); 43 | 44 | if (FilterBase::m_qV.size() < FilterBase::m_nW) 45 | { 46 | FilterBase::m_v = v; 47 | return FilterBase::m_v; 48 | } 49 | 50 | m_qSort = FilterBase::m_qV; 51 | std::sort(m_qSort.begin(), m_qSort.end()); 52 | FilterBase::m_v = m_qSort.at(m_iMid); 53 | 54 | return FilterBase::m_v; 55 | } 56 | 57 | void reset(void) 58 | { 59 | FilterBase::reset(); 60 | m_qSort.clear(); 61 | } 62 | 63 | private: 64 | int m_iMid; 65 | std::deque m_qSort; 66 | }; 67 | 68 | } 69 | #endif 70 | -------------------------------------------------------------------------------- /src/Filter/Predict.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Predict.h 3 | * 4 | * Created on: Mar 18, 2021 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Filter_Predict_H_ 9 | #define OpenKAI_src_Filter_Predict_H_ 10 | 11 | #include "FilterBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | template 17 | class Predict : public FilterBase 18 | { 19 | public: 20 | Predict() 21 | { 22 | FilterBase::m_nW = 2; 23 | m_kT = 1.0; 24 | } 25 | virtual ~Predict() 26 | { 27 | } 28 | 29 | bool init(float kT) 30 | { 31 | m_kT = kT; 32 | FilterBase::reset(); 33 | return true; 34 | } 35 | 36 | T update(T v, float dT) 37 | { 38 | FilterBase::add(v); 39 | 40 | if (FilterBase::m_qV.size() < FilterBase::m_nW) 41 | { 42 | FilterBase::m_v = v; 43 | return FilterBase::m_v; 44 | } 45 | 46 | int s = FilterBase::m_qV.size(); 47 | T p = FilterBase::m_qV.at(s - 2); 48 | T q = FilterBase::m_qV.at(s - 1); 49 | 50 | FilterBase::m_v = q + (q - p) * (T)dT * m_kT; 51 | return FilterBase::m_v; 52 | } 53 | 54 | protected: 55 | float m_kT; 56 | }; 57 | 58 | } 59 | #endif 60 | -------------------------------------------------------------------------------- /src/IO/_ADIO_EBYTE.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ADIO_EBYTE.h 3 | * 4 | * Created on: Jan 7, 2024 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_IO__ADIO_EBYTE_H_ 9 | #define OpenKAI_src_IO__ADIO_EBYTE_H_ 10 | 11 | #include "_ADIObase.h" 12 | #include "../Protocol/_Modbus.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _ADIO_EBYTE : public _ADIObase 18 | { 19 | public: 20 | _ADIO_EBYTE(); 21 | virtual ~_ADIO_EBYTE(); 22 | 23 | virtual int init(void *pKiss); 24 | virtual int link(void); 25 | virtual int start(void); 26 | virtual int check(void); 27 | virtual void close(void); 28 | virtual void console(void *pConsole); 29 | 30 | virtual bool open(void); 31 | 32 | protected: 33 | virtual void updateW(void); 34 | virtual void updateR(void); 35 | 36 | void update(void); 37 | static void *getUpdate(void *This) 38 | { 39 | ((_ADIO_EBYTE *)This)->update(); 40 | return NULL; 41 | } 42 | 43 | private: 44 | _Modbus* m_pMB; 45 | int m_iID; 46 | 47 | }; 48 | 49 | } 50 | #endif 51 | -------------------------------------------------------------------------------- /src/IO/_File.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_IO__File_H_ 2 | #define OpenKAI_src_IO__File_H_ 3 | 4 | #include "_IObase.h" 5 | 6 | using namespace std; 7 | 8 | namespace kai 9 | { 10 | 11 | class _File : public _IObase 12 | { 13 | public: 14 | _File(void); 15 | ~_File(void); 16 | 17 | int init(void *pKiss); 18 | 19 | bool open(ios_base::openmode mode = ios::in | ios::out | ios::app); 20 | bool open(const string &fName, ios_base::openmode mode = ios::in | ios::out | ios::app); 21 | void close(void); 22 | 23 | int read(uint8_t *pBuf, int nB); 24 | bool write(uint8_t *pBuf, int nB); 25 | 26 | protected: 27 | bool readAll(string *pStr); 28 | 29 | private: 30 | string m_name; 31 | string m_buf; 32 | fstream m_file; 33 | int m_iByte; 34 | }; 35 | 36 | } 37 | #endif 38 | -------------------------------------------------------------------------------- /src/IO/_SerialPort.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_IO__SerialPort_H_ 2 | #define OpenKAI_src_IO__SerialPort_H_ 3 | 4 | #include "_IObase.h" 5 | 6 | // The following two non-standard baudrates should have been defined by the system 7 | // If not, just fallback to number 8 | #ifndef B460800 9 | #define B460800 460800 10 | #endif 11 | 12 | #ifndef B921600 13 | #define B921600 921600 14 | #endif 15 | 16 | using namespace std; 17 | 18 | #define N_SERIAL_BUF 512 19 | 20 | namespace kai 21 | { 22 | 23 | class _SerialPort : public _IObase 24 | { 25 | public: 26 | _SerialPort(); 27 | ~_SerialPort(); 28 | 29 | virtual int init(void *pKiss); 30 | virtual int link(void); 31 | virtual int start(void); 32 | virtual void console(void *pConsole); 33 | 34 | virtual bool open(void); 35 | virtual void close(void); 36 | virtual int read(uint8_t *pBuf, int nB); 37 | 38 | private: 39 | bool setup(void); 40 | void update(void); 41 | static void *getUpdate(void *This) 42 | { 43 | ((_SerialPort *)This)->update(); 44 | return NULL; 45 | } 46 | 47 | private: 48 | int m_fd; 49 | string m_port; 50 | int m_baud; 51 | int m_dataBits; 52 | int m_stopBits; 53 | bool m_parity; 54 | bool m_hardwareControl; 55 | }; 56 | 57 | } 58 | #endif 59 | -------------------------------------------------------------------------------- /src/IO/_TCPclient.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _TCPclient.h 3 | * 4 | * Created on: August 8, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_IO__TCPclient_H_ 9 | #define OpenKAI_src_IO__TCPclient_H_ 10 | 11 | #include "_IObase.h" 12 | 13 | #define N_TCP_BUF 512 14 | 15 | namespace kai 16 | { 17 | 18 | class _TCPclient : public _IObase 19 | { 20 | public: 21 | _TCPclient(); 22 | virtual ~_TCPclient(); 23 | 24 | int init(void *pKiss); 25 | int start(void); 26 | void console(void *pConsole); 27 | 28 | bool open(void); 29 | void close(void); 30 | 31 | virtual int read(uint8_t *pBuf, int nB); 32 | 33 | private: 34 | void update(void); 35 | static void *getUpdate(void *This) 36 | { 37 | ((_TCPclient *)This)->update(); 38 | return NULL; 39 | } 40 | 41 | public: 42 | struct sockaddr_in m_serverAddr; 43 | string m_strAddr; 44 | uint16_t m_port; 45 | 46 | bool m_bClient; 47 | int m_socket; 48 | }; 49 | 50 | } 51 | #endif 52 | -------------------------------------------------------------------------------- /src/IO/_TCPserver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _TCPserver.h 3 | * 4 | * Created on: August 8, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_IO__TCPserver_H_ 9 | #define OpenKAI_src_IO__TCPserver_H_ 10 | 11 | #include "../Base/_ModuleBase.h" 12 | #include "../Module/Kiss.h" 13 | #include "../IO/_IObase.h" 14 | #include "_TCPclient.h" 15 | 16 | #define N_LISTEN 1 17 | #define N_SOCKET 1 18 | 19 | namespace kai 20 | { 21 | 22 | class _TCPserver : public _ModuleBase 23 | { 24 | public: 25 | _TCPserver(); 26 | virtual ~_TCPserver(); 27 | 28 | int init(void *pKiss); 29 | int start(void); 30 | void console(void *pConsole); 31 | _TCPclient *getFirstSocket(void); 32 | 33 | bool setup(void); 34 | void cleanupClient(void); 35 | bool handler(void); 36 | void update(void); 37 | static void *getUpdate(void *This) 38 | { 39 | ((_TCPserver *)This)->update(); 40 | return NULL; 41 | } 42 | 43 | public: 44 | uint16_t m_listenPort; 45 | int m_nListen; 46 | 47 | int m_socket; 48 | struct sockaddr_in m_serverAddr; 49 | list<_TCPclient *> m_lClient; 50 | unsigned int m_nSocket; 51 | }; 52 | 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /src/IO/_UDP.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _UDP.h 3 | * 4 | * Created on: June 16, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_IO__UDP_H_ 9 | #define OpenKAI_src_IO__UDP_H_ 10 | 11 | #include "_IObase.h" 12 | 13 | #define N_UDP_BUF 512 14 | 15 | namespace kai 16 | { 17 | 18 | class _UDP : public _IObase 19 | { 20 | public: 21 | _UDP(); 22 | virtual ~_UDP(); 23 | 24 | int init(void *pKiss); 25 | int start(void); 26 | void console(void *pConsole); 27 | 28 | bool open(void); 29 | void close(void); 30 | int read(uint8_t *pBuf, int nB); 31 | 32 | private: 33 | void update(void); 34 | static void *getUpdate(void *This) 35 | { 36 | ((_UDP *)This)->update(); 37 | return NULL; 38 | } 39 | 40 | protected: 41 | string m_addrRemote; 42 | uint16_t m_portRemote; 43 | uint16_t m_portLocal; 44 | bool m_bW2R; // write back to the client recevied from 45 | int m_bWbroadcast; 46 | 47 | sockaddr_in m_sAddrLocal; 48 | sockaddr_in m_sAddrRemote; 49 | int m_socket; 50 | }; 51 | 52 | } 53 | #endif 54 | -------------------------------------------------------------------------------- /src/IO/_WebSocket.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _WebSocket.cpp 3 | * 4 | * Created on: August 8, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #include "_WebSocket.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _WebSocket::_WebSocket() 14 | { 15 | m_ioType = io_webSocket; 16 | m_ioStatus = io_unknown; 17 | } 18 | 19 | _WebSocket::~_WebSocket() 20 | { 21 | m_packetR.clear(); 22 | close(); 23 | } 24 | 25 | int _WebSocket::init(void *pKiss) 26 | { 27 | Kiss *pK = (Kiss *)pKiss; 28 | 29 | int nPacket = 1024; 30 | int nPbuffer = 512; 31 | if (pK) 32 | { 33 | pK->v("nPacket", &nPacket); 34 | pK->v("nPbuffer", &nPbuffer); 35 | } 36 | 37 | IF__(!m_packetW.init(nPbuffer, nPacket), OK_ERR_ALLOCATION); 38 | IF__(!m_packetR.init(nPbuffer, nPacket), OK_ERR_ALLOCATION); 39 | 40 | return OK_OK; 41 | } 42 | 43 | int _WebSocket::read(uint8_t *pBuf, int nB) 44 | { 45 | NULL__(pBuf, 0); 46 | 47 | return m_packetR.getPacket(pBuf, nB); 48 | } 49 | 50 | IO_PACKET_FIFO *_WebSocket::getPacketFIFOr(void) 51 | { 52 | return &m_packetR; 53 | } 54 | 55 | void _WebSocket::console(void *pConsole) 56 | { 57 | NULL_(pConsole); 58 | this->_IObase::console(pConsole); 59 | } 60 | 61 | } 62 | -------------------------------------------------------------------------------- /src/IO/_WebSocket.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _webSocket.h 3 | * 4 | * Created on: August 8, 2016 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_IO__WebSocket_H_ 9 | #define OpenKAI_src_IO__WebSocket_H_ 10 | 11 | #include 12 | #include "_IObase.h" 13 | 14 | #define WS_N_BUF 512 15 | 16 | namespace kai 17 | { 18 | class _WebSocket : public _IObase 19 | { 20 | public: 21 | _WebSocket(); 22 | virtual ~_WebSocket(); 23 | 24 | int init(void *pKiss = NULL); 25 | void console(void *pConsole); 26 | 27 | int read(uint8_t *pBuf, int nB); 28 | IO_PACKET_FIFO* getPacketFIFOr(void); 29 | 30 | protected: 31 | IO_PACKET_FIFO m_packetR; 32 | 33 | }; 34 | 35 | } 36 | #endif 37 | -------------------------------------------------------------------------------- /src/IPC/SharedMem.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SharedMem.h 3 | * 4 | * Created on: Sept 20, 2022 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_IPCSharedMemBase_H_ 9 | #define OpenKAI_src_IPCSharedMemBase_H_ 10 | 11 | #include "../Base/BASE.h" 12 | #include 13 | #include 14 | #include 15 | 16 | namespace kai 17 | { 18 | 19 | class SharedMem : public BASE 20 | { 21 | public: 22 | SharedMem(); 23 | virtual ~SharedMem(); 24 | 25 | virtual int init(void *pKiss); 26 | virtual int link(void); 27 | virtual bool open(void); 28 | virtual bool bOpen(void); 29 | virtual void close(void); 30 | 31 | virtual int nB(void); 32 | virtual void* p(void); 33 | virtual bool bWriter(void); 34 | 35 | public: 36 | string m_shmName; 37 | int m_nB; 38 | int m_fd; 39 | void* m_pB; 40 | bool m_bWriter; 41 | 42 | bool m_bOpen; 43 | 44 | }; 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /src/Navigation/_GPS.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _GPS.h 3 | * 4 | * Created on: Feb 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Navigation__GPS_H_ 9 | #define OpenKAI_src_Navigation__GPS_H_ 10 | 11 | #include "../IO/_IObase.h" 12 | #include "../Dependencies/minmea.h" 13 | #include "Coordinate.h" 14 | 15 | namespace kai 16 | { 17 | 18 | class _GPS : public _ModuleBase 19 | { 20 | public: 21 | _GPS(void); 22 | virtual ~_GPS(); 23 | 24 | int init(void *pKiss); 25 | int start(void); 26 | void console(void *pConsole); 27 | 28 | LL_POS getLLpos(void); 29 | UTM_POS getUTMpos(void); 30 | 31 | private: 32 | bool readNMEA(void); 33 | void decodeNMEA(void); 34 | void update(void); 35 | static void *getUpdate(void *This) 36 | { 37 | ((_GPS *)This)->update(); 38 | return NULL; 39 | } 40 | 41 | private: 42 | _IObase *m_pIO; 43 | uint8_t m_rBuf[N_IO_BUF]; 44 | int m_nRead; 45 | int m_iRead; 46 | string m_msg; 47 | 48 | minmea_sentence_rmc m_rmc; 49 | minmea_sentence_gga m_gga; 50 | minmea_sentence_gst m_gst; 51 | minmea_sentence_gsv m_gsv; 52 | minmea_sentence_vtg m_vtg; 53 | minmea_sentence_zda m_zda; 54 | 55 | LL_POS m_LL; 56 | UTM_POS m_UTM; 57 | Coordinate m_coord; 58 | }; 59 | 60 | } 61 | #endif 62 | -------------------------------------------------------------------------------- /src/Navigation/_RStracking.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _RStracking.h 3 | * 4 | * Created on: Oct 29, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Navigation_RStracking_H_ 9 | #define OpenKAI_src_Navigation_RStracking_H_ 10 | 11 | #include "_NavBase.h" 12 | #include 13 | 14 | namespace kai 15 | { 16 | 17 | class _RStracking : public _NavBase 18 | { 19 | public: 20 | _RStracking(); 21 | virtual ~_RStracking(); 22 | 23 | int init(void *pKiss); 24 | int start(void); 25 | bool open(void); 26 | void close(void); 27 | 28 | private: 29 | bool updateRS(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_RStracking *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | void sensorReset(void); 38 | 39 | public: 40 | string m_rsSN; 41 | rs2::config m_rsConfig; 42 | rs2::pipeline m_rsPipe; 43 | rs2::pipeline_profile m_rsProfile; 44 | }; 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /src/Navigation/_RTCM3.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _RTCM3.cpp 3 | * 4 | * Created on: Jun 3, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #include "_RTCM3.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _RTCM3::_RTCM3() 14 | { 15 | m_pIO = nullptr; 16 | m_nRead = 0; 17 | m_iRead = 0; 18 | m_msg = ""; 19 | } 20 | 21 | _RTCM3::~_RTCM3() 22 | { 23 | } 24 | 25 | int _RTCM3::init(void *pKiss) 26 | { 27 | CHECK_(this->_ModuleBase::init(pKiss)); 28 | Kiss *pK = (Kiss *)pKiss; 29 | 30 | string n = ""; 31 | pK->v("_IObase", &n); 32 | m_pIO = (_IObase *)(pK->findModule(n)); 33 | NULL__(m_pIO, OK_ERR_NOT_FOUND); 34 | 35 | return OK_OK; 36 | } 37 | 38 | int _RTCM3::start(void) 39 | { 40 | NULL__(m_pT, OK_ERR_NULLPTR); 41 | return m_pT->start(getUpdate, this); 42 | } 43 | 44 | void _RTCM3::update(void) 45 | { 46 | while (m_pT->bAlive()) 47 | { 48 | m_pT->autoFPS(); 49 | 50 | decode(); 51 | m_msg = ""; 52 | 53 | } 54 | } 55 | 56 | void _RTCM3::decode(void) 57 | { 58 | } 59 | 60 | void _RTCM3::console(void *pConsole) 61 | { 62 | NULL_(pConsole); 63 | this->_ModuleBase::console(pConsole); 64 | 65 | if (!m_pIO->bOpen()) 66 | { 67 | ((_Console *)pConsole)->addMsg("Not connected"); 68 | return; 69 | } 70 | } 71 | 72 | } 73 | -------------------------------------------------------------------------------- /src/Navigation/_RTCM3.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _RTCM3.h 3 | * 4 | * Created on: Jun 3, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Navigation__RTCM3_H_ 9 | #define OpenKAI_src_Navigation__RTCM3_H_ 10 | 11 | #include "../IO/_IObase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _RTCM3 : public _ModuleBase 17 | { 18 | public: 19 | _RTCM3(void); 20 | virtual ~_RTCM3(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | void console(void *pConsole); 25 | 26 | private: 27 | void decode(void); 28 | void update(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_RTCM3 *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | private: 36 | _IObase *m_pIO; 37 | uint8_t m_rBuf[N_IO_BUF]; 38 | int m_nRead; 39 | int m_iRead; 40 | string m_msg; 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /src/Net/base64.h: -------------------------------------------------------------------------------- 1 | // 2 | // base64 encoding and decoding with C++. 3 | // Version: 2.rc.09 (release candidate) 4 | // 5 | 6 | #ifndef BASE64_H_C0CE2A47_D10E_42C9_A27C_C883944E704A 7 | #define BASE64_H_C0CE2A47_D10E_42C9_A27C_C883944E704A 8 | 9 | #include 10 | 11 | #if __cplusplus >= 201703L 12 | #include 13 | #endif // __cplusplus >= 201703L 14 | 15 | std::string base64_encode (std::string const& s, bool url = false); 16 | std::string base64_encode_pem (std::string const& s); 17 | std::string base64_encode_mime(std::string const& s); 18 | 19 | std::string base64_decode(std::string const& s, bool remove_linebreaks = false); 20 | std::string base64_encode(unsigned char const*, size_t len, bool url = false); 21 | 22 | #if __cplusplus >= 201703L 23 | // 24 | // Interface with std::string_view rather than const std::string& 25 | // Requires C++17 26 | // Provided by Yannic Bonenberger (https://github.com/Yannic) 27 | // 28 | std::string base64_encode (std::string_view s, bool url = false); 29 | std::string base64_encode_pem (std::string_view s); 30 | std::string base64_encode_mime(std::string_view s); 31 | 32 | std::string base64_decode(std::string_view s, bool remove_linebreaks = false); 33 | #endif // __cplusplus >= 201703L 34 | 35 | #endif /* BASE64_H_C0CE2A47_D10E_42C9_A27C_C883944E704A */ 36 | -------------------------------------------------------------------------------- /src/Primitive/primitiveUtil.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Primitive_primitiveUtil_H_ 2 | #define OpenKAI_src_Primitive_primitiveUtil_H_ 3 | 4 | #include 5 | #include "vFloat3.h" 6 | 7 | using namespace Eigen; 8 | 9 | namespace kai 10 | { 11 | 12 | inline Eigen::Vector3f v2e(const vFloat3 &v) 13 | { 14 | return Eigen::Vector3f(v.x, v.y, v.z); 15 | } 16 | 17 | inline Eigen::Vector3d v2e(const vDouble3 &v) 18 | { 19 | return Eigen::Vector3d(v.x, v.y, v.z); 20 | } 21 | 22 | 23 | inline vFloat3 e2v(const Eigen::Vector3f &e) 24 | { 25 | return vFloat3(e[0], e[1], e[2]); 26 | } 27 | 28 | inline vDouble3 e2v(const Eigen::Vector3d &e) 29 | { 30 | return vDouble3(e[0], e[1], e[2]); 31 | } 32 | 33 | } 34 | #endif 35 | -------------------------------------------------------------------------------- /src/Primitive/tSwap.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Primitive_tSwap_H_ 2 | #define OpenKAI_src_Primitive_tSwap_H_ 3 | 4 | namespace kai 5 | { 6 | 7 | template 8 | struct tSwap 9 | { 10 | int m_iT = 0; 11 | T m_pT[2]; 12 | 13 | void swap(void) 14 | { 15 | m_iT = 1 - m_iT; 16 | } 17 | 18 | T *get(void) 19 | { 20 | return &m_pT[m_iT]; 21 | } 22 | 23 | T *next(void) 24 | { 25 | return &m_pT[1 - m_iT]; 26 | } 27 | }; 28 | 29 | } 30 | #endif 31 | -------------------------------------------------------------------------------- /src/Primitive/vInt2.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Primitive_vInt2_H_ 2 | #define OpenKAI_src_Primitive_vInt2_H_ 3 | 4 | #include "../Base/platform.h" 5 | 6 | namespace kai 7 | { 8 | 9 | struct vInt2 10 | { 11 | int x; 12 | int y; 13 | 14 | vInt2(void) 15 | { 16 | x = 0; 17 | y = 0; 18 | } 19 | 20 | vInt2(int v) 21 | { 22 | x = v; 23 | y = v; 24 | } 25 | 26 | vInt2(int a, int b) 27 | { 28 | x = a; 29 | y = b; 30 | } 31 | 32 | void clear(void) 33 | { 34 | x = 0; 35 | y = 0; 36 | } 37 | 38 | void set(int a, int b) 39 | { 40 | x = a; 41 | y = b; 42 | } 43 | 44 | int len(void) 45 | { 46 | return y-x; 47 | } 48 | 49 | int area(void) 50 | { 51 | return abs(x * y); 52 | } 53 | 54 | int constrain(int v) 55 | { 56 | if(v < x)v=x; 57 | else if(v > y)v=y; 58 | 59 | return v; 60 | } 61 | 62 | bool bInside(int v) 63 | { 64 | IF_F(v < x); 65 | IF_F(v >= y); 66 | return true; 67 | } 68 | 69 | bool bContain(int v) 70 | { 71 | IF_F(v < x); 72 | IF_F(v > y); 73 | return true; 74 | } 75 | 76 | }; 77 | 78 | } 79 | #endif 80 | -------------------------------------------------------------------------------- /src/Primitive/vInt3.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Primitive_vInt3_H_ 2 | #define OpenKAI_src_Primitive_vInt3_H_ 3 | 4 | #include "../Base/platform.h" 5 | 6 | namespace kai 7 | { 8 | 9 | struct vInt3 10 | { 11 | int x; 12 | int y; 13 | int z; 14 | 15 | vInt3(void) 16 | { 17 | x = 0; 18 | y = 0; 19 | z = 0; 20 | } 21 | 22 | vInt3(int v) 23 | { 24 | x = v; 25 | y = v; 26 | z = v; 27 | } 28 | 29 | vInt3(int a, int b, int c) 30 | { 31 | x = a; 32 | y = b; 33 | z = c; 34 | } 35 | 36 | inline bool operator==(vInt3 r) 37 | { 38 | IF_F(x != r.x); 39 | IF_F(y != r.y); 40 | IF_F(z != r.z); 41 | 42 | return true; 43 | } 44 | 45 | void clear(void) 46 | { 47 | x = 0; 48 | y = 0; 49 | z = 0; 50 | } 51 | 52 | void set(int a, int b, int c) 53 | { 54 | x = a; 55 | y = b; 56 | z = c; 57 | } 58 | }; 59 | 60 | } 61 | #endif 62 | -------------------------------------------------------------------------------- /src/Primitive/vInt4.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Primitive_vInt4_H_ 2 | #define OpenKAI_src_Primitive_vInt4_H_ 3 | 4 | #include "../Base/platform.h" 5 | 6 | namespace kai 7 | { 8 | 9 | struct vInt4 10 | { 11 | int x; 12 | int y; 13 | int z; 14 | int w; 15 | 16 | vInt4(void) 17 | { 18 | x = 0; 19 | y = 0; 20 | z = 0; 21 | w = 0; 22 | } 23 | 24 | vInt4(int v) 25 | { 26 | x = v; 27 | y = v; 28 | z = v; 29 | w = v; 30 | } 31 | 32 | vInt4(int a, int b, int c, int d) 33 | { 34 | x = a; 35 | y = b; 36 | z = c; 37 | w = d; 38 | } 39 | 40 | void clear(void) 41 | { 42 | x = 0; 43 | y = 0; 44 | z = 0; 45 | w = 0; 46 | } 47 | 48 | void set(int v) 49 | { 50 | x = v; 51 | y = v; 52 | z = v; 53 | w = v; 54 | } 55 | 56 | void set(int a, int b, int c, int d) 57 | { 58 | x = a; 59 | y = b; 60 | z = c; 61 | w = d; 62 | } 63 | 64 | int midX(void) 65 | { 66 | return (x + z) / 2; 67 | } 68 | 69 | int midY(void) 70 | { 71 | return (y + w) / 2; 72 | } 73 | 74 | int area(void) 75 | { 76 | return abs((z - x) * (w - y)); 77 | } 78 | 79 | int width(void) 80 | { 81 | return z - x; 82 | } 83 | 84 | int height(void) 85 | { 86 | return w - y; 87 | } 88 | 89 | }; 90 | 91 | } 92 | #endif 93 | -------------------------------------------------------------------------------- /src/Protocol/_PWMio.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Protocol__PWMio_H_ 2 | #define OpenKAI_src_Protocol__PWMio_H_ 3 | 4 | #include "_ProtocolBase.h" 5 | #include "../Utility/RC.h" 6 | 7 | #define ARDU_CMD_PWM 0 8 | #define ARDU_CMD_HB 1 9 | 10 | #define PWMIO_N_CHAN 16 11 | 12 | namespace kai 13 | { 14 | 15 | class _PWMio : public _ProtocolBase 16 | { 17 | public: 18 | _PWMio(); 19 | ~_PWMio(); 20 | 21 | virtual int init(void *pKiss); 22 | virtual int start(void); 23 | virtual void console(void *pConsole); 24 | 25 | virtual void set(int iChan, uint16_t v); 26 | virtual uint16_t getRaw(int iChan); 27 | 28 | protected: 29 | virtual void send(void); 30 | virtual void handleCMD(const PROTOCOL_CMD &cmd); 31 | 32 | private: 33 | void updateW(void); 34 | static void *getUpdateW(void *This) 35 | { 36 | ((_PWMio *)This)->updateW(); 37 | return NULL; 38 | } 39 | 40 | void updateR(void); 41 | static void *getUpdateR(void *This) 42 | { 43 | ((_PWMio *)This)->updateR(); 44 | return NULL; 45 | } 46 | 47 | protected: 48 | uint8_t m_nCr; 49 | RC_CHANNEL m_pCr[PWMIO_N_CHAN]; 50 | 51 | uint8_t m_nCw; 52 | RC_CHANNEL m_pCw[PWMIO_N_CHAN]; 53 | }; 54 | 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /src/ROS/_ROS_fastLio.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ROS_fastLio.h 3 | * 4 | * Created on: Jan 7, 2024 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_ROS__ROS_fastLio_H_ 9 | #define OpenKAI_src_ROS__ROS_fastLio_H_ 10 | 11 | #include "../Navigation/_NavBase.h" 12 | #ifdef WITH_3D 13 | #include "../3D/PointCloud/_PCframe.h" 14 | #endif 15 | 16 | #include "ROS_fastLio.h" 17 | 18 | namespace kai 19 | { 20 | class _ROS_fastLio : public _NavBase 21 | { 22 | public: 23 | _ROS_fastLio(); 24 | virtual ~_ROS_fastLio(); 25 | 26 | virtual int init(void *pKiss); 27 | virtual int link(void); 28 | virtual int check(void); 29 | virtual int start(void); 30 | virtual void console(void *pConsole); 31 | 32 | private: 33 | void updateNav(void); 34 | void update(void); 35 | static void *getUpdate(void *This) 36 | { 37 | ((_ROS_fastLio *)This)->update(); 38 | return NULL; 39 | } 40 | 41 | void updateROS(void); 42 | static void *getUpdateROS(void *This) 43 | { 44 | ((_ROS_fastLio *)This)->updateROS(); 45 | return NULL; 46 | } 47 | 48 | protected: 49 | _Thread *m_pTros; 50 | 51 | shared_ptr m_pROSnode; 52 | 53 | #ifdef WITH_3D 54 | _PCframe* m_pPCframe; 55 | #endif 56 | 57 | }; 58 | 59 | } 60 | #endif 61 | -------------------------------------------------------------------------------- /src/SLAM/_SLAMbase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _SLAMbase.h 3 | * 4 | * Created on: Nov 12, 2024 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_SLAM__SLAMbase_H_ 9 | #define OpenKAI_src_SLAM__SLAMbase_H_ 10 | 11 | #include "../Vision/_VisionBase.h" 12 | #include "../Navigation/_NavBase.h" 13 | 14 | namespace kai 15 | { 16 | class _SLAMbase : public _NavBase 17 | { 18 | public: 19 | _SLAMbase(); 20 | virtual ~_SLAMbase(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int link(void); 24 | virtual int start(void); 25 | virtual int check(void); 26 | virtual void console(void *pConsole); 27 | 28 | bool bTracking(void); 29 | 30 | private: 31 | void detect(void); 32 | void update(void); 33 | static void *getUpdate(void *This) 34 | { 35 | ((_SLAMbase *)This)->update(); 36 | return NULL; 37 | } 38 | 39 | protected: 40 | _VisionBase *m_pV; 41 | vInt2 m_vSize; 42 | bool m_bTracking; 43 | 44 | }; 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /src/Science/_IsingSolver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _IsingSolver.h 3 | * 4 | * Created on: Feb 2, 2024 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Science_IsingSolver_H_ 9 | #define OpenKAI_src_Science_IsingSolver_H_ 10 | 11 | #include "../Base/_ModuleBase.h" 12 | #include "../UI/_Console.h" 13 | 14 | namespace kai 15 | { 16 | class _IsingSolver : public _ModuleBase 17 | { 18 | public: 19 | _IsingSolver(); 20 | virtual ~_IsingSolver(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int link(void); 24 | virtual int start(void); 25 | virtual int check(void); 26 | virtual void console(void *pConsole); 27 | 28 | private: 29 | void updateSolver(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_IsingSolver *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | protected: 38 | // post processing thread 39 | _Thread *m_pTPP; 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/State/StateBase.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_State_StateBase_H_ 2 | #define OpenKAI_src_State_StateBase_H_ 3 | 4 | #include "../Base/_ModuleBase.h" 5 | 6 | namespace kai 7 | { 8 | 9 | enum STATE_TYPE 10 | { 11 | state_unknown, 12 | state_base, 13 | state_land, 14 | state_loiter, 15 | state_goto, 16 | state_rth, 17 | state_wp, 18 | state_takeoff, 19 | }; 20 | 21 | class StateBase : public BASE 22 | { 23 | public: 24 | StateBase(); 25 | ~StateBase(); 26 | 27 | virtual int init(void *pKiss); 28 | virtual int link(void); 29 | virtual void console(void *pConsole); 30 | 31 | virtual void reset(void); 32 | virtual void update(void); 33 | virtual bool bComplete(void); 34 | virtual string getNext(void); 35 | virtual STATE_TYPE getType(void); 36 | 37 | virtual void updateModules(void); 38 | 39 | protected: 40 | STATE_TYPE m_type; 41 | string m_next; 42 | uint64_t m_tStart; 43 | uint64_t m_tStamp; 44 | uint64_t m_tTimeout; 45 | bool m_bComplete; 46 | 47 | vector<_ModuleBase *> m_vpModulePause; 48 | vector<_ModuleBase *> m_vpModuleResume; 49 | 50 | }; 51 | 52 | } 53 | #endif 54 | -------------------------------------------------------------------------------- /src/Tracker/_SingleTracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _SingleTracker.h 3 | * 4 | * Created on: Aug 21, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Tracker__SingleTracker_H_ 9 | #define OpenKAI_src_Tracker__SingleTracker_H_ 10 | 11 | #include "../Vision/_VisionBase.h" 12 | #include "_TrackerBase.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _SingleTracker : public _TrackerBase 18 | { 19 | public: 20 | _SingleTracker(); 21 | virtual ~_SingleTracker(); 22 | 23 | int init(void *pKiss); 24 | int start(void); 25 | 26 | void createTracker(void); 27 | 28 | private: 29 | void track(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_SingleTracker *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | public: 38 | Ptr m_pTracker; 39 | }; 40 | 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /src/Tracker/_TrackerBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _TrackerBase.h 3 | * 4 | * Created on: Aug 28, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Tracker__TrackerBase_H_ 9 | #define OpenKAI_src_Tracker__TrackerBase_H_ 10 | 11 | #include 12 | #include "../Base/_ModuleBase.h" 13 | #include "../Detector/_DetectorBase.h" 14 | #include "../Vision/_VisionBase.h" 15 | 16 | namespace kai 17 | { 18 | 19 | enum TRACK_STATE 20 | { 21 | track_init, 22 | track_update, 23 | track_stop 24 | }; 25 | 26 | class _TrackerBase : public _ModuleBase 27 | { 28 | public: 29 | _TrackerBase(); 30 | virtual ~_TrackerBase(); 31 | 32 | virtual int init(void *pKiss); 33 | virtual void update(void); 34 | virtual int check(void); 35 | virtual void draw(void *pFrame); 36 | virtual void console(void *pConsole); 37 | 38 | virtual void createTracker(void); 39 | virtual bool startTrack(vFloat4 &bb); 40 | virtual void stopTrack(void); 41 | TRACK_STATE trackState(void); 42 | vFloat4 *getBB(void); 43 | 44 | public: 45 | _VisionBase *m_pV; 46 | Rect2d m_rBB; 47 | vFloat4 m_bb; 48 | float m_margin; 49 | 50 | Rect2d m_newBB; 51 | uint64_t m_iSet; 52 | uint64_t m_iInit; 53 | 54 | string m_trackerType; 55 | TRACK_STATE m_trackState; 56 | }; 57 | 58 | } 59 | #endif 60 | -------------------------------------------------------------------------------- /src/UI/_Console.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Console.h 3 | * 4 | * Created on: Oct 13, 2019 5 | * Author: Kai Yan 6 | */ 7 | 8 | #ifndef OpenKAI_src_UI__Console_H_ 9 | #define OpenKAI_src_UI__Console_H_ 10 | 11 | #include "../Base/_ModuleBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Console : public _ModuleBase 17 | { 18 | public: 19 | _Console(); 20 | virtual ~_Console(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int link(void); 24 | virtual int start(void); 25 | 26 | void addMsg(const string &msg, int iLine = 1); 27 | void addMsg(const string &msg, int iCol, int iX, int iLine = 0); 28 | 29 | protected: 30 | void updateConsole(void); 31 | void update(void); 32 | static void *getUpdate(void *This) 33 | { 34 | ((_Console *)This)->update(); 35 | return NULL; 36 | } 37 | 38 | private: 39 | vector m_vpB; 40 | int m_iY; 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /src/UI/_GstOutput.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _GstOutput.h 3 | * 4 | * Created on: May 24, 2022 5 | * Author: Kai Yan 6 | */ 7 | 8 | #ifndef OpenKAI_src_UI_GstOutput_H_ 9 | #define OpenKAI_src_UI_GstOutput_H_ 10 | 11 | #include "_UIbase.h" 12 | #include "../Vision/Frame.h" 13 | 14 | namespace kai 15 | { 16 | class _GstOutput : public _UIbase 17 | { 18 | public: 19 | _GstOutput(); 20 | virtual ~_GstOutput(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int start(void); 24 | 25 | protected: 26 | void updateGst(void); 27 | void update(void); 28 | static void *getUpdate(void *This) 29 | { 30 | ((_GstOutput *)This)->update(); 31 | return NULL; 32 | } 33 | 34 | private: 35 | Frame m_F; 36 | vInt2 m_vSize; 37 | 38 | string m_gstOutput; 39 | VideoWriter m_gst; 40 | }; 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /src/UI/_UIbase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Window.cpp 3 | * 4 | * Created on: May 24, 2022 5 | * Author: Kai Yan 6 | */ 7 | 8 | #include "_UIbase.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _UIbase::_UIbase() 14 | { 15 | } 16 | 17 | _UIbase::~_UIbase() 18 | { 19 | } 20 | 21 | int _UIbase::init(void *pKiss) 22 | { 23 | CHECK_(this->_ModuleBase::init(pKiss)); 24 | Kiss *pK = (Kiss *)pKiss; 25 | 26 | return OK_OK; 27 | } 28 | 29 | int _UIbase::link(void) 30 | { 31 | CHECK_(this->_ModuleBase::link()); 32 | Kiss *pK = (Kiss *)m_pKiss; 33 | 34 | vector vB; 35 | pK->a("vBASE", &vB); 36 | for (string p : vB) 37 | { 38 | BASE *pB = (BASE *)(pK->findModule(p)); 39 | IF_CONT(!pB); 40 | 41 | m_vpB.push_back(pB); 42 | } 43 | 44 | return OK_OK; 45 | } 46 | 47 | int _UIbase::start(void) 48 | { 49 | NULL__(m_pT, OK_ERR_NULLPTR); 50 | return m_pT->start(getUpdate, this); 51 | } 52 | 53 | void _UIbase::update(void) 54 | { 55 | while (m_pT->bAlive()) 56 | { 57 | m_pT->autoFPS(); 58 | 59 | } 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/UI/_UIbase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _UIbase.h 3 | * 4 | * Created on: May 24, 2022 5 | * Author: Kai Yan 6 | */ 7 | 8 | #ifndef OpenKAI_src_UI_UIbase_H_ 9 | #define OpenKAI_src_UI_UIbase_H_ 10 | 11 | #include "../Base/_ModuleBase.h" 12 | 13 | namespace kai 14 | { 15 | class _UIbase : public _ModuleBase 16 | { 17 | public: 18 | _UIbase(); 19 | virtual ~_UIbase(); 20 | 21 | virtual int init(void *pKiss); 22 | virtual int link(void); 23 | virtual int start(void); 24 | 25 | protected: 26 | void update(void); 27 | static void *getUpdate(void *This) 28 | { 29 | ((_UIbase *)This)->update(); 30 | return NULL; 31 | } 32 | 33 | protected: 34 | vector m_vpB; 35 | 36 | }; 37 | } 38 | #endif 39 | -------------------------------------------------------------------------------- /src/UI/_WindowCV.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Window.h 3 | * 4 | * Created on: Dec 7, 2016 5 | * Author: Kai Yan 6 | */ 7 | 8 | #ifndef OpenKAI_src_UI_WindowCV_H_ 9 | #define OpenKAI_src_UI_WindowCV_H_ 10 | 11 | #include 12 | #include "_UIbase.h" 13 | #include "../Vision/Frame.h" 14 | #include "../Utility/utilCV.h" 15 | 16 | namespace kai 17 | { 18 | class _WindowCV : public _UIbase 19 | { 20 | public: 21 | _WindowCV(); 22 | virtual ~_WindowCV(); 23 | 24 | int init(void *pKiss); 25 | int start(void); 26 | 27 | protected: 28 | void updateWindow(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_WindowCV *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | protected: 37 | Frame m_F; 38 | vInt2 m_vSize; 39 | 40 | int m_waitKey; 41 | bool m_bFullScreen; 42 | }; 43 | 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /src/Universe/_ObjectArray.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ObjectArray.h 3 | * 4 | * Created on: June 21, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Universe__ObjectArray_H_ 9 | #define OpenKAI_src_Universe__ObjectArray_H_ 10 | 11 | #include "_Object.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _ObjectArray : public _ModuleBase 17 | { 18 | public: 19 | _ObjectArray(); 20 | virtual ~_ObjectArray(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int start(void); 24 | virtual void update(void); 25 | 26 | virtual int init(int n); 27 | 28 | //io 29 | virtual _Object *add(_Object &o); 30 | virtual _Object *get(int i); 31 | virtual void clear(void); 32 | virtual int size(void); 33 | 34 | private: 35 | static void *getUpdate(void *This) 36 | { 37 | ((_ObjectArray *)This)->update(); 38 | return NULL; 39 | } 40 | 41 | private: 42 | _Object *m_pO; 43 | int m_nBuf; 44 | int m_nO; 45 | }; 46 | 47 | } 48 | #endif 49 | -------------------------------------------------------------------------------- /src/Utility/BitFlag.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Utility_BIT_FLAG_H_ 2 | #define OpenKAI_src_Utility_BIT_FLAG_H_ 3 | 4 | #include "../Base/common.h" 5 | 6 | namespace kai 7 | { 8 | 9 | struct BIT_FLAG 10 | { 11 | uint64_t m_flag = 0; 12 | 13 | bool bClear(void) 14 | { 15 | return (m_flag == 0); 16 | } 17 | 18 | void clearAll(void) 19 | { 20 | m_flag = 0; 21 | } 22 | 23 | void set(int i) 24 | { 25 | m_flag |= (1 << i); 26 | } 27 | 28 | void clear(int i) 29 | { 30 | m_flag &= ~(1 << i); 31 | } 32 | 33 | bool b(int i, bool bClearAfterRead = false) 34 | { 35 | bool b = m_flag & (1 << i); 36 | if (bClearAfterRead) 37 | clear(i); 38 | 39 | return b; 40 | } 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /src/Utility/utilFile.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Utility_utilFile_H_ 2 | #define OpenKAI_src_Utility_utilFile_H_ 3 | 4 | #include "../Base/platform.h" 5 | #include "../Base/macro.h" 6 | #include "../Base/constant.h" 7 | 8 | namespace kai 9 | { 10 | inline bool readFile(const string &fName, string *pStr, string delim = "") 11 | { 12 | NULL_F(pStr); 13 | 14 | fstream f; 15 | f.open(fName.c_str(), ios::in); 16 | IF_F(!f.is_open()); 17 | f.seekg(0, ios_base::beg); 18 | 19 | *pStr = ""; 20 | while (f && !f.eof()) 21 | { 22 | string s; 23 | getline(f, s); 24 | *pStr += s + delim; 25 | } 26 | 27 | f.close(); 28 | 29 | return true; 30 | } 31 | 32 | inline bool writeFile(const string &fName, const string &str) 33 | { 34 | fstream f; 35 | f.open(fName.c_str(), ios::out); 36 | IF_F(!f.is_open()); 37 | 38 | f.seekg(0, ios_base::beg); 39 | 40 | IF_F(!f.write(str.c_str(), str.length())); 41 | 42 | f.flush(); 43 | f.close(); 44 | 45 | return true; 46 | } 47 | } 48 | #endif -------------------------------------------------------------------------------- /src/Utility/utilVar.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Utility_utilVar_H_ 2 | #define OpenKAI_src_Utility_utilVar_H_ 3 | 4 | namespace kai 5 | { 6 | template 7 | struct VAR_WR 8 | { 9 | T m_vR; // value read out 10 | T m_vW; // value to be written 11 | 12 | void init(T w, T r) 13 | { 14 | m_vW = w; 15 | m_vR = r; 16 | } 17 | 18 | void init(void) 19 | { 20 | m_vW = 0; 21 | m_vR = 0; 22 | } 23 | 24 | void read(T v) 25 | { 26 | m_vR = v; 27 | } 28 | 29 | void write(T v) 30 | { 31 | m_vW = v; 32 | } 33 | 34 | T getRead(void) 35 | { 36 | return m_vR; 37 | } 38 | 39 | T getWrite(void) 40 | { 41 | return m_vW; 42 | } 43 | 44 | bool bW(void) 45 | { 46 | return (m_vR != m_vW); 47 | } 48 | }; 49 | 50 | } 51 | #endif 52 | -------------------------------------------------------------------------------- /src/Vision/Frame.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Frame.h 3 | * 4 | * Created on: Aug 21, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision_Frame_H_ 9 | #define OpenKAI_src_Vision_Frame_H_ 10 | 11 | #include "FrameBase.h" 12 | #include "FrameGPU.h" 13 | 14 | namespace kai 15 | { 16 | #ifdef USE_CUDA 17 | typedef FrameGPU Frame; 18 | #else 19 | typedef FrameBase Frame; 20 | #endif 21 | } 22 | #endif 23 | -------------------------------------------------------------------------------- /src/Vision/FrameBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FrameBase.h 3 | * 4 | * Created on: Aug 21, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision_FrameBase_H_ 9 | #define OpenKAI_src_Vision_FrameBase_H_ 10 | 11 | #include "../Base/common.h" 12 | #include "../Base/cv.h" 13 | 14 | namespace kai 15 | { 16 | class FrameBase 17 | { 18 | public: 19 | FrameBase(); 20 | virtual ~FrameBase(); 21 | 22 | virtual void operator=(const Mat &m); 23 | virtual void allocate(int w, int h); 24 | virtual void copy(const FrameBase &f); 25 | virtual void copy(const Mat &m); 26 | 27 | virtual FrameBase warpAffine(Mat &mWA); 28 | virtual FrameBase flip(int iOpt); 29 | virtual FrameBase remap(const Mat &mX, const Mat &mY); 30 | virtual FrameBase crop(cv::Rect bb); 31 | virtual FrameBase resize(int w, int h); 32 | virtual FrameBase resize(double scaleW, double scaleH); 33 | virtual FrameBase cvtTo(int rType); 34 | virtual FrameBase cvtColor(int code); 35 | 36 | virtual Mat *m(void); 37 | virtual cv::Size size(void); 38 | virtual bool bEmpty(void); 39 | 40 | uint64_t tStamp(void); 41 | 42 | public: 43 | uint64_t m_tStamp; 44 | Mat m_mat; 45 | }; 46 | 47 | } 48 | #endif 49 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_ColorConvert.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ColorConvert.h 3 | * 4 | * Created on: March 12, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__ColorConvert_H_ 9 | #define OpenKAI_src_Vision__ColorConvert_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _ColorConvert : public _VisionBase 17 | { 18 | public: 19 | _ColorConvert(); 20 | virtual ~_ColorConvert(); 21 | 22 | int init(void *pKiss); 23 | int link(void); 24 | int check(void); 25 | int start(void); 26 | bool open(void); 27 | void close(void); 28 | 29 | private: 30 | void filter(void); 31 | void update(void); 32 | static void *getUpdate(void *This) 33 | { 34 | ((_ColorConvert *)This)->update(); 35 | return NULL; 36 | } 37 | 38 | public: 39 | _VisionBase *m_pV; 40 | int m_code; 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Contrast.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Contrast.h 3 | * 4 | * Created on: March 12, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Contrast_H_ 9 | #define OpenKAI_src_Vision__Contrast_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Contrast : public _VisionBase 17 | { 18 | public: 19 | _Contrast(); 20 | virtual ~_Contrast(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void filter(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_Contrast *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _VisionBase *m_pV; 38 | double m_alpha; 39 | double m_beta; 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Crop.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Crop.h 3 | * 4 | * Created on: April 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Crop_H_ 9 | #define OpenKAI_src_Vision__Crop_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Crop : public _VisionBase 17 | { 18 | public: 19 | _Crop(); 20 | virtual ~_Crop(); 21 | 22 | virtual int init(void *pKiss); 23 | virtual int link(void); 24 | virtual int start(void); 25 | virtual bool open(void); 26 | virtual void close(void); 27 | 28 | private: 29 | void filter(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_Crop *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | public: 38 | _VisionBase *m_pV; 39 | vInt4 m_vRoi; 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Depth2Gray.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Depth2Gray.h 3 | * 4 | * Created on: April 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Depth2Gray_H_ 9 | #define OpenKAI_src_Vision__Depth2Gray_H_ 10 | 11 | #include "../RGBD/_RGBDbase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Depth2Gray : public _VisionBase 17 | { 18 | public: 19 | _Depth2Gray(); 20 | virtual ~_Depth2Gray(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void filter(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_Depth2Gray *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _RGBDbase *m_pV; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_DepthProj.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DepthProj.h 3 | * 4 | * Created on: June 15, 2022 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__DepthProj_H_ 9 | #define OpenKAI_src_Vision__DepthProj_H_ 10 | 11 | #include "../RGBD/_RGBDbase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _DepthProj : public _VisionBase 17 | { 18 | public: 19 | _DepthProj(); 20 | virtual ~_DepthProj(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | void createFilterMat(void); 28 | 29 | private: 30 | void filter(void); 31 | void update(void); 32 | static void *getUpdate(void *This) 33 | { 34 | ((_DepthProj *)This)->update(); 35 | return NULL; 36 | } 37 | 38 | public: 39 | _RGBDbase *m_pV; 40 | Mat m_mF; 41 | 42 | float m_fFov; 43 | vFloat2 m_vCenter; 44 | }; 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_DepthShow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DepthShow.h 3 | * 4 | * Created on: April 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__RSdepth_H_ 9 | #define OpenKAI_src_Vision__RSdepth_H_ 10 | 11 | #include "../RGBD/_RGBDbase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _DepthShow : public _VisionBase 17 | { 18 | public: 19 | _DepthShow(); 20 | virtual ~_DepthShow(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void filter(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_DepthShow *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _RGBDbase *m_pV; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Erode.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Erode.h 3 | * 4 | * Created on: March 12, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Erode_H_ 9 | #define OpenKAI_src_Vision__Erode_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | struct IMG_ERODE 17 | { 18 | int m_nItr; 19 | int m_kShape; 20 | int m_kW; 21 | int m_kH; 22 | int m_aX; 23 | int m_aY; 24 | Mat m_kernel; 25 | 26 | void init(void) 27 | { 28 | m_nItr = 1; 29 | m_kW = 3; 30 | m_kH = 3; 31 | m_aX = -1; 32 | m_aY = -1; 33 | m_kShape = cv::MORPH_RECT; 34 | } 35 | 36 | void updateKernel(void) 37 | { 38 | m_kernel = getStructuringElement(m_kShape, cv::Size(m_kW, m_kH), cv::Point(m_aX, m_aY)); 39 | } 40 | }; 41 | 42 | class _Erode : public _VisionBase 43 | { 44 | public: 45 | _Erode(); 46 | virtual ~_Erode(); 47 | 48 | int init(void *pKiss); 49 | int start(void); 50 | bool open(void); 51 | void close(void); 52 | 53 | private: 54 | void filter(void); 55 | void update(void); 56 | static void *getUpdate(void *This) 57 | { 58 | ((_Erode *)This)->update(); 59 | return NULL; 60 | } 61 | 62 | public: 63 | _VisionBase *m_pV; 64 | Frame m_fIn; 65 | vector m_vFilter; 66 | }; 67 | 68 | } 69 | #endif 70 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_HistEqualize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _HistEqualize.h 3 | * 4 | * Created on: March 12, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__HistEqualize_H_ 9 | #define OpenKAI_src_Vision__HistEqualize_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _HistEqualize : public _VisionBase 17 | { 18 | public: 19 | _HistEqualize(); 20 | virtual ~_HistEqualize(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void filter(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_HistEqualize *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _VisionBase *m_pV; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_InRange.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _InRange.h 3 | * 4 | * Created on: April 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__InRange_H_ 9 | #define OpenKAI_src_Vision__InRange_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _InRange : public _VisionBase 17 | { 18 | public: 19 | _InRange(); 20 | virtual ~_InRange(); 21 | 22 | int init(void *pKiss); 23 | int link(void); 24 | int start(void); 25 | bool open(void); 26 | void close(void); 27 | 28 | private: 29 | void filter(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_InRange *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | public: 38 | _VisionBase *m_pV; 39 | vInt3 m_vL; 40 | vInt3 m_vH; 41 | }; 42 | 43 | } 44 | #endif 45 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Invert.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Invert.h 3 | * 4 | * Created on: March 14, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Invert_H_ 9 | #define OpenKAI_src_Vision__Invert_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Invert : public _VisionBase 17 | { 18 | public: 19 | _Invert(); 20 | virtual ~_Invert(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void filter(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_Invert *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _VisionBase *m_pV; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Mask.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Mask.h 3 | * 4 | * Created on: July 2, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Mask_H_ 9 | #define OpenKAI_src_Vision__Mask_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Mask : public _VisionBase 17 | { 18 | public: 19 | _Mask(); 20 | virtual ~_Mask(); 21 | 22 | int init(void *pKiss); 23 | int link(void); 24 | int start(void); 25 | bool open(void); 26 | void close(void); 27 | 28 | private: 29 | void filter(void); 30 | void update(void); 31 | static void *getUpdate(void *This) 32 | { 33 | ((_Mask *)This)->update(); 34 | return NULL; 35 | } 36 | 37 | public: 38 | _VisionBase *m_pV; 39 | _VisionBase *m_pVmask; 40 | 41 | Frame m_fIn; 42 | Frame m_fMask; 43 | }; 44 | 45 | } 46 | #endif 47 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Remap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Remap.h 3 | * 4 | * Created on: May 7, 2021 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Remap_H_ 9 | #define OpenKAI_src_Vision__Remap_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Remap : public _VisionBase 17 | { 18 | public: 19 | _Remap(); 20 | virtual ~_Remap(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | bool bReady(void); 27 | 28 | bool setCamMat(const Mat &mC, const Mat &mD); 29 | bool scaleCamMat(void); 30 | // void updateCamMat(void); 31 | vDouble2 getF(void); 32 | vDouble2 getC(void); 33 | vFloat2 getFf(void); 34 | vFloat2 getCf(void); 35 | Mat mC(void); 36 | Mat mCscaled(void); 37 | Mat mD(void); 38 | 39 | private: 40 | void filter(void); 41 | void update(void); 42 | static void *getUpdate(void *This) 43 | { 44 | ((_Remap *)This)->update(); 45 | return NULL; 46 | } 47 | 48 | public: 49 | _VisionBase *m_pV; 50 | bool m_bReady; 51 | string m_fCalib; 52 | 53 | //original camera matrix 54 | Mat m_mC; //Intrinsic 55 | Mat m_mCscaled; //scaled with input image size 56 | Mat m_mD; //Distortion 57 | //Remap 58 | Mat m_m1; 59 | Mat m_m2; 60 | }; 61 | 62 | } 63 | #endif 64 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Resize.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Resize.h 3 | * 4 | * Created on: April 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Resize_H_ 9 | #define OpenKAI_src_Vision__Resize_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Resize : public _VisionBase 17 | { 18 | public: 19 | _Resize(); 20 | virtual ~_Resize(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void filter(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_Resize *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _VisionBase *m_pV; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/Vision/ImgFilter/_Rotate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Rotate.h 3 | * 4 | * Created on: April 23, 2019 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Rotate_H_ 9 | #define OpenKAI_src_Vision__Rotate_H_ 10 | 11 | #include "../_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Rotate : public _VisionBase 17 | { 18 | public: 19 | _Rotate(); 20 | virtual ~_Rotate(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void filter(void); 29 | void update(void); 30 | static void *getUpdate(void *This) 31 | { 32 | ((_Rotate *)This)->update(); 33 | return NULL; 34 | } 35 | 36 | public: 37 | _VisionBase *m_pV; 38 | int m_code; 39 | }; 40 | 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /src/Vision/_CamCalib.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_src_Application_CamCalib__CamCalib_H_ 2 | #define OpenKAI_src_Application_CamCalib__CamCalib_H_ 3 | 4 | #include "../Base/_ModuleBase.h" 5 | 6 | namespace kai 7 | { 8 | class _CamCalib : public _ModuleBase 9 | { 10 | public: 11 | _CamCalib(); 12 | ~_CamCalib(); 13 | 14 | int init(void *pKiss); 15 | int start(void); 16 | 17 | bool calibRGB(const char *pPath); 18 | Mat mC(void); 19 | Mat mD(void); 20 | 21 | protected: 22 | void update(void); 23 | static void *getUpdate(void *This) 24 | { 25 | ((_CamCalib *)This)->update(); 26 | return NULL; 27 | } 28 | 29 | public: 30 | string m_path; 31 | vInt2 m_vChessBoardSize; //col, row 32 | float m_squareSize; 33 | 34 | Mat m_mC; 35 | Mat m_mD; 36 | }; 37 | 38 | } 39 | #endif 40 | -------------------------------------------------------------------------------- /src/Vision/_Camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Camera.h 3 | * 4 | * Created on: Aug 22, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Camera_H_ 9 | #define OpenKAI_src_Vision__Camera_H_ 10 | 11 | #include "_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _Camera : public _VisionBase 17 | { 18 | public: 19 | _Camera(); 20 | virtual ~_Camera(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void update(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_Camera *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | public: 36 | int m_deviceID; 37 | VideoCapture m_camera; 38 | int m_nInitRead; 39 | bool m_bResetCam; 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/Vision/_DenseFlow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _DenseFlow.h 3 | * 4 | * Created on: Aug 21, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__Flow_H_ 9 | #define OpenKAI_src_Vision__Flow_H_ 10 | 11 | #include "../Base/common.h" 12 | #include "_VisionBase.h" 13 | 14 | namespace kai 15 | { 16 | 17 | class _DenseFlow : public _ModuleBase 18 | { 19 | public: 20 | _DenseFlow(); 21 | virtual ~_DenseFlow(); 22 | 23 | int init(void *pKiss); 24 | int start(void); 25 | void draw(void *pFrame); 26 | 27 | vDouble2 vFlow(vInt4 *pROI); 28 | vDouble2 vFlow(vDouble4 *pROI); 29 | 30 | private: 31 | bool isFlowCorrect(Point2f u); 32 | Vec3b computeColor(float fx, float fy); 33 | void drawOpticalFlow(const Mat_ &flowx, const Mat_ &flowy, Mat &dst, float maxmotion); 34 | void detect(void); 35 | void update(void); 36 | static void *getUpdate(void *This) 37 | { 38 | ((_DenseFlow *)This)->update(); 39 | return NULL; 40 | } 41 | 42 | public: 43 | int m_w; 44 | int m_h; 45 | _VisionBase *m_pVision; 46 | // FrameGroup *m_pGrayFrames; 47 | Ptr m_pFarn; 48 | GpuMat m_gFlow; 49 | Mat m_pFlow[2]; 50 | 51 | int m_nHistLev; 52 | vDouble2 m_vRange; 53 | double m_minHistD; 54 | }; 55 | 56 | } 57 | #endif 58 | -------------------------------------------------------------------------------- /src/Vision/_GPhoto.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _GPhoto.cpp 3 | * 4 | * Created on: Feb 20, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #include "_GPhoto.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _GPhoto::_GPhoto() 14 | { 15 | m_type = vision_gphoto; 16 | 17 | m_cmdUnmount = "gio mount -s gphoto2"; 18 | } 19 | 20 | _GPhoto::~_GPhoto() 21 | { 22 | close(); 23 | } 24 | 25 | int _GPhoto::init(void *pKiss) 26 | { 27 | CHECK_(_VisionBase::init(pKiss)); 28 | Kiss *pK = (Kiss *)pKiss; 29 | 30 | pK->v("mount", &m_cmdUnmount); 31 | 32 | return OK_OK; 33 | } 34 | 35 | bool _GPhoto::open(void) 36 | { 37 | if (!m_cmdUnmount.empty()) 38 | system(m_cmdUnmount.c_str()); 39 | 40 | return true; 41 | } 42 | 43 | void _GPhoto::close(void) 44 | { 45 | this->_VisionBase::close(); 46 | } 47 | 48 | int _GPhoto::start(void) 49 | { 50 | NULL__(m_pT, OK_ERR_NULLPTR); 51 | return m_pT->start(getUpdate, this); 52 | } 53 | 54 | void _GPhoto::update(void) 55 | { 56 | while (m_pT->bAlive()) 57 | { 58 | if (!m_bOpen) 59 | { 60 | if (!open()) 61 | { 62 | m_pT->sleepT(SEC_2_USEC); 63 | continue; 64 | } 65 | } 66 | 67 | m_pT->autoFPS(); 68 | 69 | } 70 | } 71 | 72 | bool _GPhoto::shutter(string &fName) 73 | { 74 | return true; 75 | } 76 | 77 | } 78 | -------------------------------------------------------------------------------- /src/Vision/_GPhoto.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _GPhoto.h 3 | * 4 | * Created on: Feb 20, 2020 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__GPhoto_H_ 9 | #define OpenKAI_src_Vision__GPhoto_H_ 10 | 11 | #include "_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _GPhoto: public _VisionBase 17 | { 18 | public: 19 | _GPhoto(); 20 | virtual ~_GPhoto(); 21 | 22 | int init(void* pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | bool shutter(string& fName); 28 | 29 | private: 30 | void update(void); 31 | static void* getUpdate(void* This) 32 | { 33 | ((_GPhoto*) This)->update(); 34 | return NULL; 35 | } 36 | 37 | public: 38 | string m_cmdUnmount; 39 | 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/Vision/_GStreamer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _Gstreamer.h 3 | * 4 | * Created on: Dec 21, 2017 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__GStreamer_H_ 9 | #define OpenKAI_src_Vision__GStreamer_H_ 10 | 11 | #include "_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _GStreamer : public _VisionBase 17 | { 18 | public: 19 | _GStreamer(); 20 | virtual ~_GStreamer(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void update(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_GStreamer *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | public: 36 | string m_pipeline; 37 | VideoCapture m_gst; 38 | int m_nInitRead; 39 | }; 40 | 41 | } 42 | #endif 43 | -------------------------------------------------------------------------------- /src/Vision/_ImgFile.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * _ImgFile.cpp 3 | * 4 | * Created on: Aug 5, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #include "_ImgFile.h" 9 | 10 | namespace kai 11 | { 12 | 13 | _ImgFile::_ImgFile() 14 | { 15 | m_type = vision_file; 16 | m_file = ""; 17 | } 18 | 19 | _ImgFile::~_ImgFile() 20 | { 21 | } 22 | 23 | int _ImgFile::init(void *pKiss) 24 | { 25 | CHECK_(_VisionBase::init(pKiss)); 26 | Kiss *pK = (Kiss *)pKiss; 27 | 28 | pK->v("file", &m_file); 29 | 30 | return OK_OK; 31 | } 32 | 33 | bool _ImgFile::open(void) 34 | { 35 | Mat m = imread(m_file); 36 | if (m.empty()) 37 | { 38 | LOG_E("Cannot open file: " + m_file); 39 | return false; 40 | } 41 | 42 | m_fRGB.copy(m); 43 | m_vSizeRGB.x = m.cols; 44 | m_vSizeRGB.y = m.rows; 45 | 46 | m_bOpen = true; 47 | return true; 48 | } 49 | 50 | int _ImgFile::start(void) 51 | { 52 | NULL__(m_pT, OK_ERR_NULLPTR); 53 | return m_pT->start(getUpdate, this); 54 | } 55 | 56 | void _ImgFile::update(void) 57 | { 58 | while (m_pT->bAlive()) 59 | { 60 | if (!m_bOpen) 61 | { 62 | if (!open()) 63 | { 64 | m_pT->sleepT(SEC_2_USEC); 65 | continue; 66 | } 67 | } 68 | 69 | m_pT->autoFPS(); 70 | 71 | } 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /src/Vision/_ImgFile.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _ImgFile.h 3 | * 4 | * Created on: Aug 5, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__ImgFile_H_ 9 | #define OpenKAI_src_Vision__ImgFile_H_ 10 | 11 | #include "_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _ImgFile : public _VisionBase 17 | { 18 | public: 19 | _ImgFile(); 20 | virtual ~_ImgFile(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | 25 | private: 26 | bool open(void); 27 | void update(void); 28 | static void *getUpdate(void *This) 29 | { 30 | ((_ImgFile *)This)->update(); 31 | return NULL; 32 | } 33 | 34 | public: 35 | string m_file; 36 | }; 37 | 38 | } 39 | #endif 40 | -------------------------------------------------------------------------------- /src/Vision/_InfiRay.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _InfiRay.h 3 | * 4 | * Created on: Aug 22, 2015 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__InfiRay_H_ 9 | #define OpenKAI_src_Vision__InfiRay_H_ 10 | 11 | #include "_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _InfiRay : public _VisionBase 17 | { 18 | public: 19 | _InfiRay(); 20 | virtual ~_InfiRay(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | bool open(void); 25 | void close(void); 26 | 27 | private: 28 | void update(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_InfiRay *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | public: 36 | int m_deviceID; 37 | VideoCapture m_InfiRay; 38 | int m_nInitRead; 39 | bool m_bResetCam; 40 | }; 41 | 42 | } 43 | #endif 44 | -------------------------------------------------------------------------------- /src/Vision/_SharedMemImg.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _SharedMemImg.h 3 | * 4 | * Created on: Sept 20, 2022 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision_SharedMemImg_H_ 9 | #define OpenKAI_src_Vision_SharedMemImg_H_ 10 | 11 | #include "_VisionBase.h" 12 | #include "../Utility/util.h" 13 | #include "../IPC/SharedMem.h" 14 | 15 | namespace kai 16 | { 17 | 18 | class _SharedMemImg : public _VisionBase 19 | { 20 | public: 21 | _SharedMemImg(); 22 | virtual ~_SharedMemImg(); 23 | 24 | virtual int init(void *pKiss); 25 | virtual int link(void); 26 | virtual int start(void); 27 | virtual int check(void); 28 | 29 | private: 30 | void update(void); 31 | bool update_SharedMemImg(void); 32 | static void *getUpdate(void *This) 33 | { 34 | ((_SharedMemImg *)This)->update(); 35 | return NULL; 36 | } 37 | 38 | public: 39 | SharedMem* m_pSHM; 40 | int m_matType; 41 | 42 | }; 43 | 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /src/Vision/_VideoFile.h: -------------------------------------------------------------------------------- 1 | /* 2 | * _VideoFile.h 3 | * 4 | * Created on: Aug 5, 2018 5 | * Author: yankai 6 | */ 7 | 8 | #ifndef OpenKAI_src_Vision__VideoFile_H_ 9 | #define OpenKAI_src_Vision__VideoFile_H_ 10 | 11 | #include "_VisionBase.h" 12 | 13 | namespace kai 14 | { 15 | 16 | class _VideoFile : public _VisionBase 17 | { 18 | public: 19 | _VideoFile(); 20 | virtual ~_VideoFile(); 21 | 22 | int init(void *pKiss); 23 | int start(void); 24 | void close(void); 25 | 26 | private: 27 | bool open(void); 28 | void update(void); 29 | static void *getUpdate(void *This) 30 | { 31 | ((_VideoFile *)This)->update(); 32 | return NULL; 33 | } 34 | 35 | public: 36 | VideoCapture m_vc; 37 | string m_videoFile; 38 | }; 39 | 40 | } 41 | #endif 42 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "OpenKAI.h" 2 | #include "Module/Kiss.h" 3 | #include "Utility/utilFile.h" 4 | 5 | using namespace kai; 6 | 7 | int main(int argc, char* argv[]) 8 | { 9 | if(argc < 2) 10 | { 11 | printf("Usage: ./OpenKAI [kiss file]\n"); 12 | return 0; 13 | } 14 | 15 | string argStr(argv[1]); 16 | if(argStr == "-h" || argStr == "--help") 17 | { 18 | printf("Usage: ./OpenKAI [kiss file]\n"); 19 | return 0; 20 | } 21 | 22 | printf("Kiss file: %s\n", argStr.c_str()); 23 | 24 | OpenKAI* pOK = new OpenKAI(); 25 | 26 | if(pOK->init() != OK_OK) 27 | goto exit; 28 | 29 | if(pOK->addKiss(argStr) != OK_OK) 30 | { 31 | printf("Kiss file read failed: %s\n", argStr.c_str()); 32 | goto exit; 33 | } 34 | 35 | if(pOK->createAllModules() != OK_OK) 36 | goto exit; 37 | 38 | if(pOK->initAllModules() != OK_OK) 39 | goto exit; 40 | 41 | if(pOK->linkAllModules() != OK_OK) 42 | goto exit; 43 | 44 | if(pOK->startAllModules() != OK_OK) 45 | goto exit; 46 | 47 | 48 | pOK->waitForComplete(); 49 | 50 | exit: 51 | delete pOK; 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /test/IO/_TestWebSocket.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_test__TestWebSocket_H_ 2 | #define OpenKAI_test__TestWebSocket_H_ 3 | 4 | #include "../_TestBase.h" 5 | #include "../../src/IO/_WebSocketServer.h" 6 | 7 | namespace kai 8 | { 9 | 10 | class _TestWebSocket : public _TestBase 11 | { 12 | public: 13 | _TestWebSocket(); 14 | ~_TestWebSocket(); 15 | 16 | virtual int init(void *pKiss); 17 | virtual int link(void); 18 | virtual int start(void); 19 | virtual int check(void); 20 | virtual void console(void *pConsole); 21 | 22 | protected: 23 | void update(void); 24 | static void *getUpdate(void *This) 25 | { 26 | ((_TestWebSocket *)This)->update(); 27 | return NULL; 28 | } 29 | 30 | protected: 31 | _WebSocketServer* m_pWSserver; 32 | }; 33 | 34 | } 35 | #endif 36 | -------------------------------------------------------------------------------- /test/Protocol/_TestJSON.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_test__TestJSON_H_ 2 | #define OpenKAI_test__TestJSON_H_ 3 | 4 | #include "../_TestBase.h" 5 | #include "../../src/Protocol/_JSONbase.h" 6 | 7 | namespace kai 8 | { 9 | 10 | class _TestJSON : public _TestBase 11 | { 12 | public: 13 | _TestJSON(); 14 | ~_TestJSON(); 15 | 16 | virtual int init(void *pKiss); 17 | virtual int link(void); 18 | virtual int start(void); 19 | virtual int check(void); 20 | virtual void console(void *pConsole); 21 | 22 | protected: 23 | void update(void); 24 | static void *getUpdate(void *This) 25 | { 26 | ((_TestJSON *)This)->update(); 27 | return NULL; 28 | } 29 | 30 | protected: 31 | _JSONbase* m_pJsender; 32 | _JSONbase* m_pJreceiver; 33 | }; 34 | 35 | } 36 | #endif 37 | -------------------------------------------------------------------------------- /test/_TestBase.cpp: -------------------------------------------------------------------------------- 1 | #include "_TestBase.h" 2 | 3 | namespace kai 4 | { 5 | 6 | _TestBase::_TestBase() 7 | { 8 | } 9 | 10 | _TestBase::~_TestBase() 11 | { 12 | } 13 | 14 | int _TestBase::init(void *pKiss) 15 | { 16 | CHECK_(this->_ModuleBase::init(pKiss)); 17 | Kiss *pK = (Kiss *)pKiss; 18 | 19 | // pK->v("", &); 20 | 21 | return OK_OK; 22 | } 23 | 24 | int _TestBase::link(void) 25 | { 26 | CHECK_(this->_ModuleBase::link()); 27 | 28 | return OK_OK; 29 | } 30 | 31 | int _TestBase::start(void) 32 | { 33 | NULL__(m_pT, OK_ERR_NULLPTR); 34 | return m_pT->start(getUpdate, this); 35 | } 36 | 37 | int _TestBase::check(void) 38 | { 39 | return this->_ModuleBase::check(); 40 | } 41 | 42 | void _TestBase::update(void) 43 | { 44 | while (m_pT->bAlive()) 45 | { 46 | m_pT->autoFPS(); 47 | 48 | 49 | 50 | } 51 | } 52 | 53 | void _TestBase::console(void *pConsole) 54 | { 55 | NULL_(pConsole); 56 | this->_ModuleBase::console(pConsole); 57 | 58 | // string msg; 59 | // ((_Console *)pConsole)->addMsg(msg, 1); 60 | } 61 | 62 | } 63 | -------------------------------------------------------------------------------- /test/_TestBase.h: -------------------------------------------------------------------------------- 1 | #ifndef OpenKAI_test__TestBase_H_ 2 | #define OpenKAI_test__TestBase_H_ 3 | 4 | #include "../src/Base/_ModuleBase.h" 5 | #include "../src/UI/_Console.h" 6 | 7 | namespace kai 8 | { 9 | 10 | class _TestBase : public _ModuleBase 11 | { 12 | public: 13 | _TestBase(); 14 | ~_TestBase(); 15 | 16 | virtual int init(void *pKiss); 17 | virtual int link(void); 18 | virtual int start(void); 19 | virtual int check(void); 20 | virtual void console(void *pConsole); 21 | 22 | protected: 23 | void update(void); 24 | static void *getUpdate(void *This) 25 | { 26 | ((_TestBase *)This)->update(); 27 | return NULL; 28 | } 29 | 30 | protected: 31 | 32 | }; 33 | 34 | } 35 | #endif 36 | --------------------------------------------------------------------------------