| 27 | { |
| 28 | |
| 29 | ArucoSensorModel::ArucoSensorModel(rclcpp::Node & node) |
| 30 | { |
| 31 | covariance_ = node.declare_parameter<std::vector<double>>( |
| 32 | "sensors.aruco.covariance", {0.025, 0.025}); |
| 33 | timeout_ = node.declare_parameter<double>("sensors.aruco.measurement_timeout", 0.1); |
| 34 | |
| 35 | tags_ = { |
| 36 | // side 1 |
| 37 | {0, TagLocation{0.6096, 0, -M_PI_2}}, |
| 38 | {1, TagLocation{0.6096, 0.11, -M_PI_2}}, |
| 39 | {2, TagLocation{0.6096, 0.22, -M_PI_2}}, |
| 40 | {3, TagLocation{0.6096, 0.33, -M_PI_2}}, |
| 41 | {4, TagLocation{0.6096, -0.11, -M_PI_2}}, |
| 42 | {5, TagLocation{0.6096, -0.22, -M_PI_2}}, |
| 43 | {6, TagLocation{0.6096, -0.33, -M_PI_2}}, |
| 44 | // side 2 |
| 45 | {7, TagLocation{0.0, -0.381, M_PI}}, |
| 46 | {8, TagLocation{0.11, -0.381, M_PI}}, |
| 47 | {9, TagLocation{0.22, -0.381, M_PI}}, |
| 48 | {13, TagLocation{0.33, -0.381, M_PI}}, |
| 49 | {14, TagLocation{0.44, -0.381, M_PI}}, |
| 50 | {15, TagLocation{0.55, -0.381, M_PI}}, |
| 51 | {16, TagLocation{-0.11, -0.381, M_PI}}, |
| 52 | {17, TagLocation{-0.22, -0.381, M_PI}}, |
| 53 | {18, TagLocation{-0.33, -0.381, M_PI}}, |
| 54 | {19, TagLocation{-0.44, -0.381, M_PI}}, |
| 55 | {20, TagLocation{-0.55, -0.381, M_PI}}, |
| 56 | // side 3 |
| 57 | {21, TagLocation{-0.6096, 0, M_PI_2}}, |
| 58 | {22, TagLocation{-0.6096, 0.11, M_PI_2}}, |
| 59 | {23, TagLocation{-0.6096, 0.22, M_PI_2}}, |
| 60 | {24, TagLocation{-0.6096, 0.33, M_PI_2}}, |
| 61 | {25, TagLocation{-0.6096, -0.11, M_PI_2}}, |
| 62 | {26, TagLocation{-0.6096, -0.22, M_PI_2}}, |
| 63 | {27, TagLocation{-0.6096, -0.33, M_PI_2}}, |
| 64 | // side 4 |
| 65 | {28, TagLocation{0.0, 0.381, 0}}, |
| 66 | {29, TagLocation{0.11, 0.381, 0}}, |
| 67 | {30, TagLocation{0.22, 0.381, 0}}, |
| 68 | {31, TagLocation{0.33, 0.381, 0}}, |
| 69 | {32, TagLocation{0.44, 0.381, 0}}, |
| 70 | {33, TagLocation{0.55, 0.381, 0}}, |
| 71 | {34, TagLocation{-0.11, 0.381, 0}}, |
| 72 | {35, TagLocation{-0.22, 0.381, 0}}, |
| 73 | {36, TagLocation{-0.33, 0.381, 0}}, |
| 74 | {37, TagLocation{-0.44, 0.381, 0}}, |
| 75 | {38, TagLocation{-0.55, 0.381, 0}}, |
| 76 | }; |
| 77 | |
| 78 | tag_sub_ = node.create_subscription<stsl_interfaces::msg::TagArray>( |
| 79 | "/tags", 1, std::bind(&ArucoSensorModel::UpdateMeasurement, this, std::placeholders::_1)); |
| 80 | } |
| 81 | |
| 82 | void ArucoSensorModel::UpdateMeasurement(const stsl_interfaces::msg::TagArray::SharedPtr msg) |
| 83 | { |
nothing calls this directly
no outgoing calls
no test coverage detected