|
9 | 9 | namespace depthai_ros_driver { |
10 | 10 | namespace param_handlers { |
11 | 11 | ImuParamHandler::ImuParamHandler(std::shared_ptr<rclcpp::Node> node, const std::string& name) : BaseParamHandler(node, name) { |
12 | | - imuSyncMethodMap = { |
| 12 | + syncMethodMap = { |
13 | 13 | {"COPY", dai::ros::ImuSyncMethod::COPY}, |
14 | 14 | {"LINEAR_INTERPOLATE_GYRO", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_GYRO}, |
15 | 15 | {"LINEAR_INTERPOLATE_ACCEL", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL}, |
16 | 16 | }; |
17 | | - imuMessagetTypeMap = { |
| 17 | + messagetTypeMap = { |
18 | 18 | {"IMU", imu::ImuMsgType::IMU}, {"IMU_WITH_MAG", imu::ImuMsgType::IMU_WITH_MAG}, {"IMU_WITH_MAG_SPLIT", imu::ImuMsgType::IMU_WITH_MAG_SPLIT}}; |
19 | 19 | rotationVectorTypeMap = {{"ROTATION_VECTOR", dai::IMUSensor::ROTATION_VECTOR}, |
20 | 20 | {"GAME_ROTATION_VECTOR", dai::IMUSensor::GAME_ROTATION_VECTOR}, |
21 | 21 | {"GEOMAGNETIC_ROTATION_VECTOR", dai::IMUSensor::GEOMAGNETIC_ROTATION_VECTOR}, |
22 | 22 | {"ARVR_STABILIZED_ROTATION_VECTOR", dai::IMUSensor::ARVR_STABILIZED_ROTATION_VECTOR}, |
23 | 23 | {"ARVR_STABILIZED_GAME_ROTATION_VECTOR", dai::IMUSensor::ARVR_STABILIZED_GAME_ROTATION_VECTOR}}; |
| 24 | + accelerometerModeMap = {{"ACCELEROMETER_RAW", dai::IMUSensor::ACCELEROMETER_RAW}, |
| 25 | + {"ACCELEROMETER", dai::IMUSensor::ACCELEROMETER}, |
| 26 | + {"LINEAR_ACCELERATION", dai::IMUSensor::LINEAR_ACCELERATION}, |
| 27 | + {"GRAVITY", dai::IMUSensor::GRAVITY}}; |
| 28 | + gyroscopeModeMap = {{"GYROSCOPE_RAW", dai::IMUSensor::GYROSCOPE_RAW}, |
| 29 | + {"GYROSCOPE_CALIBRATED", dai::IMUSensor::GYROSCOPE_CALIBRATED}, |
| 30 | + {"GYROSCOPE_UNCALIBRATED", dai::IMUSensor::GYROSCOPE_UNCALIBRATED}}; |
| 31 | + magnetometerModeMap = {{"MAGNETOMETER_RAW", dai::IMUSensor::MAGNETOMETER_RAW}, |
| 32 | + {"MAGNETOMETER_CALIBRATED", dai::IMUSensor::MAGNETOMETER_CALIBRATED}, |
| 33 | + {"MAGNETOMETER_UNCALIBRATED", dai::IMUSensor::MAGNETOMETER_UNCALIBRATED}}; |
24 | 34 | } |
25 | 35 | ImuParamHandler::~ImuParamHandler() = default; |
26 | 36 | void ImuParamHandler::declareParams(std::shared_ptr<dai::node::IMU> imu, const std::string& imuType) { |
27 | 37 | declareAndLogParam<bool>("i_get_base_device_timestamp", false); |
28 | 38 | declareAndLogParam<int>("i_max_q_size", 8); |
29 | 39 | auto messageType = declareAndLogParam<std::string>("i_message_type", "IMU"); |
30 | 40 | declareAndLogParam<std::string>("i_sync_method", "LINEAR_INTERPOLATE_ACCEL"); |
31 | | - declareAndLogParam<float>("i_acc_cov", 0.0); |
32 | | - declareAndLogParam<float>("i_gyro_cov", 0.0); |
33 | | - declareAndLogParam<float>("i_rot_cov", -1.0); |
34 | | - declareAndLogParam<float>("i_mag_cov", 0.0); |
35 | 41 | declareAndLogParam<bool>("i_update_ros_base_time_on_ros_msg", false); |
36 | | - bool rotationAvailable = imuType == "BNO086"; |
37 | | - if(declareAndLogParam<bool>("i_enable_rotation", false)) { |
| 42 | + if(declareAndLogParam<bool>("i_enable_acc", true)) { |
| 43 | + const std::string accelerometerModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_acc_mode", "ACCELEROMETER_RAW")); |
| 44 | + const dai::IMUSensor accelerometerMode = utils::getValFromMap(accelerometerModeName, accelerometerModeMap); |
| 45 | + const int accelerometerFreq = declareAndLogParam<int>("i_acc_freq", 400); |
| 46 | + declareAndLogParam<float>("i_acc_cov", 0.0); |
| 47 | + |
| 48 | + imu->enableIMUSensor(accelerometerMode, accelerometerFreq); |
| 49 | + } |
| 50 | + |
| 51 | + if(declareAndLogParam<bool>("i_enable_gyro", true)) { |
| 52 | + const std::string gyroscopeModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_gyro_mode", "GYROSCOPE_RAW")); |
| 53 | + const dai::IMUSensor gyroscopeMode = utils::getValFromMap(gyroscopeModeName, gyroscopeModeMap); |
| 54 | + const int gyroscopeFreq = declareAndLogParam<int>("i_gyro_freq", 400); |
| 55 | + declareAndLogParam<float>("i_gyro_cov", 0.0); |
| 56 | + |
| 57 | + imu->enableIMUSensor(gyroscopeMode, gyroscopeFreq); |
| 58 | + } |
| 59 | + |
| 60 | + const bool magnetometerAvailable = imuType == "BNO086"; |
| 61 | + if(declareAndLogParam<bool>("i_enable_mag", magnetometerAvailable)) { |
| 62 | + if(magnetometerAvailable) { |
| 63 | + const std::string magnetometerModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_mag_mode", "MAGNETOMETER_RAW")); |
| 64 | + const dai::IMUSensor magnetometerMode = utils::getValFromMap(magnetometerModeName, magnetometerModeMap); |
| 65 | + const int magnetometerFreq = declareAndLogParam<int>("i_mag_freq", 100); |
| 66 | + declareAndLogParam<float>("i_mag_cov", 0.0); |
| 67 | + |
| 68 | + imu->enableIMUSensor(magnetometerMode, magnetometerFreq); |
| 69 | + } else { |
| 70 | + RCLCPP_ERROR(getROSNode()->get_logger(), "Magnetometer enabled but not available with current sensor"); |
| 71 | + declareAndLogParam<bool>("i_enable_mag", false, true); |
| 72 | + } |
| 73 | + } |
| 74 | + |
| 75 | + const bool rotationAvailable = imuType == "BNO086"; |
| 76 | + if(declareAndLogParam<bool>("i_enable_rotation", rotationAvailable)) { |
38 | 77 | if(rotationAvailable) { |
39 | | - auto rotationVecType = utils::getValFromMap(utils::getUpperCaseStr(declareAndLogParam<std::string>("i_rotation_vector_type", "ROTATION_VECTOR")), |
40 | | - rotationVectorTypeMap); |
41 | | - imu->enableIMUSensor(rotationVecType, declareAndLogParam<int>("i_rot_freq", 400)); |
42 | | - // if imu message type is IMU_WITH_MAG or IMU_WITH_MAG_SPLIT, enable magnetometer |
43 | | - if(messageType == "IMU_WITH_MAG" || messageType == "IMU_WITH_MAG_SPLIT") { |
44 | | - imu->enableIMUSensor(dai::IMUSensor::MAGNETOMETER_CALIBRATED, declareAndLogParam<int>("i_mag_freq", 100)); |
45 | | - } |
| 78 | + const std::string rotationModeName = utils::getUpperCaseStr(declareAndLogParam<std::string>("i_rot_mode", "ROTATION_VECTOR")); |
| 79 | + const dai::IMUSensor rotationMode = utils::getValFromMap(rotationModeName, rotationVectorTypeMap); |
| 80 | + const int rotationFreq = declareAndLogParam<int>("i_rot_freq", 400); |
| 81 | + declareAndLogParam<float>("i_rot_cov", -1.0); |
| 82 | + |
| 83 | + imu->enableIMUSensor(rotationMode, rotationFreq); |
46 | 84 | } else { |
47 | 85 | RCLCPP_ERROR(getROSNode()->get_logger(), "Rotation enabled but not available with current sensor"); |
48 | 86 | declareAndLogParam<bool>("i_enable_rotation", false, true); |
49 | 87 | } |
| 88 | + imu->setBatchReportThreshold(declareAndLogParam<int>("i_batch_report_threshold", 5)); |
| 89 | + imu->setMaxBatchReports(declareAndLogParam<int>("i_max_batch_reports", 10)); |
50 | 90 | } |
51 | | - imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, declareAndLogParam<int>("i_acc_freq", 400)); |
52 | | - imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, declareAndLogParam<int>("i_gyro_freq", 400)); |
53 | | - imu->setBatchReportThreshold(declareAndLogParam<int>("i_batch_report_threshold", 5)); |
54 | | - imu->setMaxBatchReports(declareAndLogParam<int>("i_max_batch_reports", 10)); |
55 | 91 | } |
56 | 92 |
|
57 | 93 | dai::ros::ImuSyncMethod ImuParamHandler::getSyncMethod() { |
58 | | - return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_sync_method")), imuSyncMethodMap); |
| 94 | + return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_sync_method")), syncMethodMap); |
59 | 95 | } |
60 | 96 |
|
61 | 97 | imu::ImuMsgType ImuParamHandler::getMsgType() { |
62 | | - return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_message_type")), imuMessagetTypeMap); |
| 98 | + return utils::getValFromMap(utils::getUpperCaseStr(getParam<std::string>("i_message_type")), messagetTypeMap); |
63 | 99 | } |
64 | 100 |
|
65 | 101 | dai::CameraControl ImuParamHandler::setRuntimeParams(const std::vector<rclcpp::Parameter>& /*params*/) { |
|
0 commit comments