diff --git a/CMakeLists.txt b/CMakeLists.txt index 73b05b6..bb6f877 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,6 @@ endforeach() # Boost find_package(Boost 1.74.0 COMPONENTS REQUIRED - # Insert desired libraries below (see /usr/include/boost/ for available options) serialization system thread diff --git a/config/README.md b/config/README.md index bafc22d..74fa697 100644 --- a/config/README.md +++ b/config/README.md @@ -7,25 +7,25 @@ description at the top describing its purpose. To run with pure default defined in the code, run: -``` +```shell ros2 launch network_systems main_launch.py ``` To run with config files in this folder: -``` +```shell ros2 launch network_systems main_launch.py config:= ``` For example: -``` +```shell ros2 launch network_systems main_launch.py config:=default_prod_en.yaml ``` launches network_systems with the parameters specified in `default_prod_en.yaml`. -``` +```shell ros2 launch network_systems main_launch.py config:=default.yaml,example/example_en.yaml ``` @@ -34,7 +34,7 @@ launches network_systems with the parameters specified in `default_prod_en.yaml` **NOTE**: Instead of defining a `mode` parameter for each node, a global ROS launch argument is used. -``` +```shell ros2 launch network_systems main_launch.py config:=<...> mode:=production ros2 launch network_systems main_launch.py config:=<...> mode:=development ``` diff --git a/config/default_prod_en.yaml b/config/default_prod_en.yaml index e473dd6..35fb08a 100644 --- a/config/default_prod_en.yaml +++ b/config/default_prod_en.yaml @@ -16,6 +16,10 @@ network_systems: ros__parameters: enabled: true + mock_ais_node: + ros__parameters: + enabled: false + remote_transceiver_node: ros__parameters: enabled: true diff --git a/config/mock_ais/mock_ais_en_default.yaml b/config/mock_ais/mock_ais_en_default.yaml new file mode 100644 index 0000000..04c8b4d --- /dev/null +++ b/config/mock_ais/mock_ais_en_default.yaml @@ -0,0 +1,5 @@ +# Enable Mock AIS with default settings +network_systems: + mock_ais_node: + ros__parameters: + enabled: true diff --git a/config/mock_ais/mock_ais_template.yaml b/config/mock_ais/mock_ais_template.yaml new file mode 100644 index 0000000..34290af --- /dev/null +++ b/config/mock_ais/mock_ais_template.yaml @@ -0,0 +1,11 @@ +# Template for the Mock AIS module +network_systems: + mock_ais_node: + ros__parameters: + enabled: true + # The following parameters are optional. Defaults are set in mock_ais_ros_intf.cpp + publish_rate_ms: # Integer: How frequently the Mock AIS publishes data in milliseconds (ex. 500) + seed: # Integer: Random seed used for random data generation + num_sim_ships: # Integer: Total number of AIS ships to simulate (does not include Polaris) + polaris_start_pos: # Integer Array (size 2): Initial latitude and longitude of Polaris for simulation + # (ex. [49.283, -123.652]) diff --git a/launch/main_launch.py b/launch/main_launch.py index 171e5d4..dc5cac4 100644 --- a/launch/main_launch.py +++ b/launch/main_launch.py @@ -72,6 +72,7 @@ def setup_launch(context: LaunchContext) -> List[Node]: """ launch_description_entities = list() launch_description_entities.append(get_cached_fib_description(context)) + launch_description_entities.append(get_mock_ais_description(context)) launch_description_entities.append(get_remote_transceiver_description(context)) return launch_description_entities @@ -88,6 +89,7 @@ def get_cached_fib_description(context: LaunchContext) -> Node: node_name = "cached_fib_subscriber" ros_parameters = [ global_launch_config, + {"mode": LaunchConfiguration("mode")}, *LaunchConfiguration("config").perform(context).split(","), ] ros_arguments: List[SomeSubstitutionsType] = [ @@ -107,6 +109,38 @@ def get_cached_fib_description(context: LaunchContext) -> Node: return node +def get_mock_ais_description(context: LaunchContext) -> Node: + """Gets the launch description for the mock_ais node. + + Args: + context (LaunchContext): The current launch context. + + Returns: + Node: The node object that launches the mock_ais node. + """ + node_name = "mock_ais_node" + ros_parameters = [ + global_launch_config, + {"mode": LaunchConfiguration("mode")}, + *LaunchConfiguration("config").perform(context).split(","), + ] + ros_arguments: List[SomeSubstitutionsType] = [ + "--log-level", + [f"{node_name}:=", LaunchConfiguration("log_level")], + ] + + node = Node( + package=PACKAGE_NAME, + namespace=NAMESPACE, + executable="mock_ais", + name=node_name, + parameters=ros_parameters, + ros_arguments=ros_arguments, + ) + + return node + + def get_remote_transceiver_description(context: LaunchContext) -> Node: """Gets the launch description for the remote_transceiver node. @@ -114,13 +148,13 @@ def get_remote_transceiver_description(context: LaunchContext) -> Node: context (LaunchContext): The current launch context. Returns: - Node: The node object that launches the cached_fib node. + Node: The node object that launches the remote_transceiver node. """ node_name = "remote_transceiver_node" ros_parameters = [ global_launch_config, - *LaunchConfiguration("config").perform(context).split(","), {"mode": LaunchConfiguration("mode")}, + *LaunchConfiguration("config").perform(context).split(","), ] ros_arguments: List[SomeSubstitutionsType] = [ "--log-level", diff --git a/lib/cmn_hdrs/shared_constants.h b/lib/cmn_hdrs/shared_constants.h index 4c935e0..e4a23b3 100644 --- a/lib/cmn_hdrs/shared_constants.h +++ b/lib/cmn_hdrs/shared_constants.h @@ -35,18 +35,20 @@ constexpr float HEADING_LBND = 0.0; constexpr float HEADING_UBND = 360.0; // boat rotation -constexpr float ROT_LBND = -360.0; -constexpr float ROT_UBND = 360; +// See https://documentation.spire.com/ais-fundamentals/rate-of-turn-rot/ for how ROT works +constexpr int8_t ROT_LBND = -126; +constexpr int8_t ROT_UBND = 126; + // boat dimension -constexpr float DIMENSION_LBND = 0; -constexpr float DIMENSION_UBND = 650.0; +constexpr float SHIP_DIMENSION_LBND = 1; // arbitrary number +constexpr float SHIP_DIMENSION_UBND = 650.0; // arbitrary number /***** Bounds for Battery ******/ -constexpr float VOLT_LBND = 0.5; // Placeholder number -constexpr float VOLT_UBND = 250.0; // Placeholder number -constexpr float CURRENT_LBND = -200.0; // Placeholder number -constexpr float CURRENT_UBND = 200.0; // Placeholder number +constexpr float BATT_VOLT_LBND = 0.5; // Placeholder number +constexpr float BATT_VOLT_UBND = 250.0; // Placeholder number +constexpr float BATT_CURR_LBND = -200.0; // Placeholder number +constexpr float BATT_CURR_UBND = 200.0; // Placeholder number /***** Bounds for Wind Sensor ******/ -constexpr int DIRECTION_LBND = -180; -constexpr int DIRECTION_UBND = 179; +constexpr int WIND_DIRECTION_LBND = -180; +constexpr int WIND_DIRECTION_UBND = 179; diff --git a/projects/CMakeLists.txt b/projects/CMakeLists.txt index 21ec35a..2f187bf 100755 --- a/projects/CMakeLists.txt +++ b/projects/CMakeLists.txt @@ -1,3 +1,4 @@ +add_subdirectory(mock_ais) add_subdirectory(can_transceiver) add_subdirectory(example) add_subdirectory(local_transceiver) diff --git a/projects/mock_ais/CMakeLists.txt b/projects/mock_ais/CMakeLists.txt new file mode 100644 index 0000000..837c8d7 --- /dev/null +++ b/projects/mock_ais/CMakeLists.txt @@ -0,0 +1,31 @@ +set(module mock_ais) + +# define external dependencies with link_libs and inc_dirs variables +set(link_libs +) + +set(inc_dirs +) + +set(compile_defs +) + +# Create module library +set(srcs + ${CMAKE_CURRENT_LIST_DIR}/src/mock_ais.cpp +) +make_lib(${module} "${srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}") + +# Create module ROS executable +set(bin_srcs + ${srcs} + ${CMAKE_CURRENT_LIST_DIR}/src/mock_ais_ros_intf.cpp +) +make_exe(${module} "${bin_srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}") + +# Create unit test +set(test_srcs + ${srcs} + ${CMAKE_CURRENT_LIST_DIR}/test/test_mock_ais.cpp +) +make_unit_test(${module} "${test_srcs}" "${link_libs}" "${inc_dirs}" "${compile_defs}") diff --git a/projects/mock_ais/inc/mock_ais.h b/projects/mock_ais/inc/mock_ais.h new file mode 100644 index 0000000..0d6ea86 --- /dev/null +++ b/projects/mock_ais/inc/mock_ais.h @@ -0,0 +1,202 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace qvm = boost::qvm; + +/** + * @brief Convenience struct for a 2D floating point vector + * All functionality is inherited from qvm::vec + */ +struct Vec2DFloat : public qvm::vec +{ + // Explicitly use the member variable 'a' in the base qvm::vec class + using qvm::vec::a; + + /** + * @brief Instantiate an empty Vec2DFloat + * + */ + Vec2DFloat() {} + + /** + * @brief Instantiate a Vec2DFloat with initial component values + * + * @param i + * @param j + */ + Vec2DFloat(float i, float j) : qvm::vec{i, j} {} + + /** + * @brief Convenience array accessor for a + * + * @param idx + * @return float& + */ + float & operator[](std::size_t idx) { return a[idx]; } + + /** + * @brief Convenience array accessor for a + * + * @param idx + * @return const float& + */ + const float & operator[](std::size_t idx) const { return a[idx]; } +}; + +// NOLINTBEGIN +// This boost::qvm:: namespace segment is just boilerplate to register the Vec2DFloat type with qvm's vector operations. +// See https://www.boost.org/doc/libs/1_74_0/libs/qvm/doc/html/index.html#vec_traits for more info. +// Even though its boilerplate, it triggers linter errors so disable linting for this section. +namespace boost +{ +namespace qvm +{ +template <> +struct vec_traits +{ + static int const dim = 2; + + using scalar_type = float; + + template + static inline scalar_type & write_element(Vec2DFloat & v) + { + return v.a[I]; + } + + template + static inline scalar_type read_element(Vec2DFloat const & v) + { + return v.a[I]; + } +}; +} // namespace qvm +} // namespace boost +// NOLINTEND +namespace defaults +{ +constexpr float MAX_HEADING_CHANGE = 2.0; // Max degree change per tick +constexpr float MAX_SPEED_CHANGE = 1.0; // Min degree change per tick +constexpr float MIN_AIS_SHIP_DIST = 0.001; // Min 111m (at equator) distance of ais ships from Polaris +constexpr float MAX_AIS_SHIP_DIST = 0.1; // Max 11.1km (at equator) distance of ais ships from Polaris +constexpr int MIN_AIS_SHIP_WIDTH_M = 2; // A boat this small likely won't have AIS +constexpr int MAX_AIS_SHIP_WIDTH_M = 49; // Typical container ship width +// Minimum and maximum ratios pulled from: http://marine.marsh-design.com/content/length-beam-ratio +constexpr int MIN_AIS_SHIP_L_W_RATIO = 2; // Ship length should be at least 2x width +constexpr int MAX_AIS_SHIP_L_W_RATIO = 16; // Ship length should be at most 16x width +constexpr int UPDATE_RATE_MS = 500; // Update frequency +constexpr int SEED = 123456; // Randomization seed +constexpr int NUM_SIM_SHIPS = 20; // Number of ais ships to simulate +const Vec2DFloat POLARIS_START_POS{49.28397458822112, -123.6525841364974}; // some point in the Strait of Georgia; +} // namespace defaults + +/** + * @brief Struct that mirrors the definition of custom_interfaces::msg::HelperAISShip + * + */ +struct AisShip +{ + Vec2DFloat lat_lon_; + float speed_; + float heading_; + uint32_t id_; + uint32_t width_; + uint32_t length_; + int8_t rot_; +}; + +/** + * @brief Extra per ship simulation parameters + */ +struct SimShipConfig +{ + float max_heading_change_; // Max degree change per tick + float max_speed_change_; // Min degree change per tick + float max_ship_dist_; // Maximum distance from Polaris + float min_ship_dist_; // Minimum distance from Polaris + uint32_t min_ship_width_m_; // Minimum ship width in meters + uint32_t max_ship_width_m_; // Maximum ship width in meters + uint32_t min_ship_l_w_ratio_; // Minimum ship length:width ratio + uint32_t max_ship_l_w_ratio_; // Maximum ship length:width ratio +}; + +class MockAisShip : public AisShip +{ +public: + /** + * @brief Construct a new Mock Ais Ship object + * + * @param seed Random seed + * @param id ID of the new sim ship + * @param polaris_lat_lon Position of Poalris + * @param config Sim ship constraints + */ + MockAisShip(uint32_t seed, uint32_t id, Vec2DFloat polaris_lat_lon, SimShipConfig config); + + /** + * @brief Update the AIS Ship instance + * + * @param polaris_lat_lon Current position of Polaris + */ + void tick(const Vec2DFloat & polaris_lat_lon); + +private: + std::mt19937 mt_rng_; // Random number generator + SimShipConfig config_; // Sim ship constraints +}; + +/** + * Simulate Ais Ships + */ +class MockAis +{ +public: + /** + * @brief Construct a new Mock AIS simulation + * + * @param seed Random seeds + * @param num_ships Number of sim ships to spawn + * @param polaris_lat_lon Position of Polaris + */ + MockAis(uint32_t seed, uint32_t num_ships, Vec2DFloat polaris_lat_lon); + + /** + * @brief Construct a new Mock AIS simulation + * + * @param seed Random seeds + * @param num_ships Number of sim ships to spawn + * @param polaris_lat_lon Position of Polaris + * @param config Extra sim ship constraints + */ + MockAis(uint32_t seed, uint32_t num_ships, Vec2DFloat polaris_lat_lon, SimShipConfig config); + + /** + * @brief Get the current AIS ships + * + * @return A vector of AisShip objects + */ + std::vector ships() const; + + /** + * @brief Update the current position of Polaris + * + * @param lat_lon Polaris' position + */ + void updatePolarisPos(const Vec2DFloat & lat_lon); + + /** + * @brief Update every simulated AIS ship + * + */ + void tick(); + +private: + Vec2DFloat polaris_lat_lon_; // Polaris' current position + std::vector ships_; // Vector of all simulated Ais ships +}; diff --git a/projects/mock_ais/src/mock_ais.cpp b/projects/mock_ais/src/mock_ais.cpp new file mode 100644 index 0000000..0b6265e --- /dev/null +++ b/projects/mock_ais/src/mock_ais.cpp @@ -0,0 +1,156 @@ +#include "mock_ais.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "cmn_hdrs/shared_constants.h" + +namespace +{ +/** + * @brief Convert degress to radians + * + * @param degrees + * @return radians + */ +float degToRad(const float & degrees) +{ + return static_cast(degrees * M_PI / 180.0); // NOLINT(readability-magic-numbers) +} + +/** + * @brief Bound a heading to our current limits + * + * @param heading + * @return bounded heading + */ +float boundHeading(const float & heading) +{ + if (heading < HEADING_LBND) { + return heading + HEADING_UBND; + } + if (heading >= HEADING_UBND) { + return heading - HEADING_UBND; + } + return heading; +} + +/** + * @brief Convert a heading to a 2D direction unit vector + * + * @param heading + * @return unit direction vector + */ +Vec2DFloat headingToVec2D(const float & heading) +{ + float angle = degToRad(heading); + // Since 0 is north (y-axis), use sin for x and cos for y + return {std::sin(angle), std::cos(angle)}; +} +} // namespace + +MockAisShip::MockAisShip(uint32_t seed, uint32_t id, Vec2DFloat polaris_lat_lon, SimShipConfig config) +: mt_rng_(seed), config_(config) +{ + static const std::array pos_or_neg = {-1.0, 1.0}; + std::uniform_real_distribution lat_dist(config_.min_ship_dist_, config_.max_ship_dist_); + std::uniform_real_distribution lon_dist(config_.min_ship_dist_, config_.max_ship_dist_); + std::uniform_real_distribution speed_dist(SPEED_LBND, SPEED_UBND); + std::uniform_real_distribution heading_dist(HEADING_LBND, HEADING_UBND); + std::uniform_int_distribution pos_or_neg_dist(0, 1); + std::uniform_int_distribution beam_dist(config_.min_ship_width_m_, config_.max_ship_width_m_); + std::uniform_int_distribution l_w_ratio_dist(config_.min_ship_l_w_ratio_, config_.max_ship_l_w_ratio_); + std::uniform_int_distribution rot_dist(ROT_LBND, ROT_UBND); + + id_ = id; + lat_lon_ = { + polaris_lat_lon[0] + pos_or_neg[pos_or_neg_dist(mt_rng_)] * lat_dist(mt_rng_), + polaris_lat_lon[1] + pos_or_neg[pos_or_neg_dist(mt_rng_)] * lon_dist(mt_rng_)}; + speed_ = speed_dist(mt_rng_); + heading_ = heading_dist(mt_rng_); + width_ = beam_dist(mt_rng_); + length_ = width_ * l_w_ratio_dist(mt_rng_); + rot_ = rot_dist(mt_rng_); +} + +void MockAisShip::tick(const Vec2DFloat & polaris_lat_lon) +{ + std::uniform_real_distribution heading_dist( + heading_ - config_.max_heading_change_, heading_ + config_.max_heading_change_); + std::uniform_real_distribution speed_dist( + speed_ - config_.max_speed_change_, speed_ + config_.max_speed_change_); + std::uniform_int_distribution rot_dist(ROT_LBND, ROT_UBND); + + float speed = speed_dist(mt_rng_); + if (speed > SPEED_UBND) { + speed = SPEED_UBND; + } else if (speed < SPEED_LBND) { + speed = SPEED_LBND; + } + + heading_ = boundHeading(heading_dist(mt_rng_)); + speed_ = speed; + Vec2DFloat dir_vec = headingToVec2D(heading_); + Vec2DFloat displacement = dir_vec * speed_; + Vec2DFloat new_pos = lat_lon_ + displacement; + Vec2DFloat polaris_to_new_pos_vec = new_pos - polaris_lat_lon; + float polaris_to_new_pos_distance = qvm::mag(polaris_to_new_pos_vec); + + if (polaris_to_new_pos_distance < config_.min_ship_dist_) { + qvm::normalize(polaris_to_new_pos_vec); + lat_lon_ = polaris_lat_lon + config_.min_ship_dist_ * polaris_to_new_pos_vec; + } else if (polaris_to_new_pos_distance > config_.max_ship_dist_) { + qvm::normalize(polaris_to_new_pos_vec); + lat_lon_ = polaris_lat_lon + config_.max_ship_dist_ * polaris_to_new_pos_vec; + } else { + lat_lon_ = new_pos; + } + + // ROT does not necessarily depend on actual change in heading, so we can use a random number + rot_ = rot_dist(mt_rng_); +} + +MockAis::MockAis(uint32_t seed, uint32_t num_ships, Vec2DFloat polaris_lat_lon) +: MockAis( + seed, num_ships, polaris_lat_lon, + {.max_heading_change_ = defaults::MAX_HEADING_CHANGE, + .max_speed_change_ = defaults::MAX_SPEED_CHANGE, + .max_ship_dist_ = defaults::MAX_AIS_SHIP_DIST, + .min_ship_dist_ = defaults::MIN_AIS_SHIP_DIST, + .min_ship_width_m_ = defaults::MIN_AIS_SHIP_WIDTH_M, + .max_ship_width_m_ = defaults::MAX_AIS_SHIP_WIDTH_M, + .min_ship_l_w_ratio_ = defaults::MIN_AIS_SHIP_L_W_RATIO, + .max_ship_l_w_ratio_ = defaults::MAX_AIS_SHIP_L_W_RATIO}) +{ +} + +MockAis::MockAis(uint32_t seed, uint32_t num_ships, Vec2DFloat polaris_lat_lon, SimShipConfig config) +: polaris_lat_lon_(polaris_lat_lon) +{ + for (uint32_t i = 0; i < num_ships; i++) { + ships_.push_back(MockAisShip(seed + i, i, polaris_lat_lon, config)); + } +} + +std::vector MockAis::ships() const +{ + std::vector ships; + for (const MockAisShip & ship : ships_) { + ships.push_back(ship); + } + return ships; +} + +void MockAis::updatePolarisPos(const Vec2DFloat & lat_lon) { polaris_lat_lon_ = lat_lon; } + +void MockAis::tick() +{ + for (MockAisShip & ship : ships_) { + ship.tick(polaris_lat_lon_); + } +} diff --git a/projects/mock_ais/src/mock_ais_ros_intf.cpp b/projects/mock_ais/src/mock_ais_ros_intf.cpp new file mode 100644 index 0000000..2fd0832 --- /dev/null +++ b/projects/mock_ais/src/mock_ais_ros_intf.cpp @@ -0,0 +1,125 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cmn_hdrs/ros_info.h" +#include "cmn_hdrs/shared_constants.h" +#include "mock_ais.h" + +/** + * Connect the Mock AIS to the onbaord ROS network + */ +class MockAisRosIntf : public rclcpp::Node +{ +public: + MockAisRosIntf() : Node("mock_ais_node") + { + static constexpr int ROS_Q_SIZE = 5; + this->declare_parameter("enabled", false); + + if (this->get_parameter("enabled").as_bool()) { + this->declare_parameter("mode", rclcpp::PARAMETER_STRING); + this->declare_parameter("publish_rate_ms", defaults::UPDATE_RATE_MS); + this->declare_parameter("seed", defaults::SEED); + this->declare_parameter("num_sim_ships", defaults::NUM_SIM_SHIPS); + this->declare_parameter( + "polaris_start_pos", + std::vector({defaults::POLARIS_START_POS[0], defaults::POLARIS_START_POS[1]})); + + rclcpp::Parameter mode_param = this->get_parameter("mode"); + rclcpp::Parameter publish_rate_ms_param = this->get_parameter("publish_rate_ms"); + rclcpp::Parameter seed_param = this->get_parameter("seed"); + rclcpp::Parameter num_sim_ships_param = this->get_parameter("num_sim_ships"); + rclcpp::Parameter polaris_start_pos_param = this->get_parameter("polaris_start_pos"); + + std::string mode = mode_param.as_string(); + int64_t publish_rate_ms = publish_rate_ms_param.as_int(); + int64_t seed = seed_param.as_int(); + int64_t num_sim_ships = num_sim_ships_param.as_int(); + Vec2DFloat polaris_start_pos = {// annoyingly ugly type conversion :/ + static_cast(polaris_start_pos_param.as_double_array()[0]), + static_cast(polaris_start_pos_param.as_double_array()[1])}; + + RCLCPP_INFO( + this->get_logger(), + "Running Mock AIS in mode: %s, with publish_rate_ms: %s, seed: %s, num_ships %s, polaris_start_pos: %s", + mode.c_str(), publish_rate_ms_param.value_to_string().c_str(), seed_param.value_to_string().c_str(), + num_sim_ships_param.value_to_string().c_str(), polaris_start_pos_param.value_to_string().c_str()); + + // TODO(): Add ROS parameters so that we can use the MockAis constructor that takes SimShipConfig + // Optionally use nested parameters: https://answers.ros.org/question/325939/declare-nested-parameter/ + mock_ais_ = std::make_unique(seed, num_sim_ships, polaris_start_pos); + std::string polaris_gps_topic = mode == SYSTEM_MODE::DEV ? MOCK_GPS_TOPIC : GPS_TOPIC; + + // The subscriber callback is very simple so it's just the following lambda function + sub_ = this->create_subscription( + polaris_gps_topic, ROS_Q_SIZE, [&mock_ais_ = mock_ais_](custom_interfaces::msg::GPS mock_gps) { + mock_ais_->updatePolarisPos({mock_gps.lat_lon.latitude, mock_gps.lat_lon.longitude}); + }); + + pub_ = this->create_publisher(AIS_SHIPS_TOPIC, ROS_Q_SIZE); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(publish_rate_ms), std::bind(&MockAisRosIntf::pubShipsCB, this)); + } else { + RCLCPP_INFO(this->get_logger(), "Mock AIS is DISABLED"); + } + } + +private: + std::unique_ptr mock_ais_; // Mock AIS instance + rclcpp::TimerBase::SharedPtr timer_; // publish timer + rclcpp::Publisher::SharedPtr pub_; // Publish new AISShips info + rclcpp::Subscription::SharedPtr sub_; // Subscribe to Polaris' GPS coordinates + + /** + * @brief Publish the latest mock ais ships data + * + */ + void pubShipsCB() + { + mock_ais_->tick(); + std::vector ais_ships = mock_ais_->ships(); + custom_interfaces::msg::AISShips msg{}; + for (const AisShip & ais_ship : ais_ships) { + custom_interfaces::msg::HelperAISShip helper_ship; + helper_ship.set__id(static_cast(ais_ship.id_)); + custom_interfaces::msg::HelperHeading helper_head; + helper_head.set__heading(ais_ship.heading_); + helper_ship.set__cog(helper_head); + custom_interfaces::msg::HelperSpeed helper_speed; + helper_speed.set__speed(ais_ship.speed_); + helper_ship.set__sog(helper_speed); + custom_interfaces::msg::HelperLatLon lat_lon; + lat_lon.set__latitude(ais_ship.lat_lon_[0]); + lat_lon.set__longitude(ais_ship.lat_lon_[1]); + helper_ship.set__lat_lon(lat_lon); + custom_interfaces::msg::HelperDimension width; + width.set__dimension(static_cast(ais_ship.width_)); + helper_ship.set__width(width); + custom_interfaces::msg::HelperDimension length; + length.set__dimension(static_cast(ais_ship.length_)); + helper_ship.set__length(length); + custom_interfaces::msg::HelperROT rot; + rot.set__rot(ais_ship.rot_); + helper_ship.set__rot(rot); + + msg.ships.push_back(helper_ship); + } + pub_->publish(msg); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/projects/mock_ais/test/test_mock_ais.cpp b/projects/mock_ais/test/test_mock_ais.cpp new file mode 100644 index 0000000..6098b25 --- /dev/null +++ b/projects/mock_ais/test/test_mock_ais.cpp @@ -0,0 +1,152 @@ +#include + +#include +#include +#include +#include +#include + +#include "cmn_hdrs/shared_constants.h" +#include "mock_ais.h" + +using defaults::MAX_AIS_SHIP_DIST; +using defaults::MAX_HEADING_CHANGE; +using defaults::MAX_SPEED_CHANGE; +using defaults::MIN_AIS_SHIP_DIST; + +constexpr uint32_t NUM_SHIPS = 50; +constexpr uint32_t NUM_TEST_CYCLES = 100; +const Vec2DFloat POLARIS_START_POS = defaults::POLARIS_START_POS; + +// Floating point equality is a pain with rounding. For the Mock AIS, GoogleTest's default precision for floating point +// quality is much stricter than we need. Since there's no built-in API to adjust the absolute error for <= and >=, +// these two functions implement that functionality. See: +// http://google.github.io/googletest/advanced.html#floating-point-comparison +// http://google.github.io/googletest/reference/assertions.html#EXPECT_PRED_FORMAT +constexpr float MAX_FLOAT_ERR = 0.00001; // Very imprecise, but good enough for us +testing::AssertionResult AssertFloatGE(const char * m_expr, const char * n_expr, int m, int n) +{ + if ((m > n) || (std::fabs(m - n) < MAX_FLOAT_ERR)) { + return testing::AssertionSuccess(); + } + + return testing::AssertionFailure() << "Expected: " << m_expr << " >= " << n_expr << std::endl + << " Actual: " << m << " vs " << n << std::endl + << "Using max floating point error of: " << MAX_FLOAT_ERR << std::endl; +} +testing::AssertionResult AssertFloatLE(const char * m_expr, const char * n_expr, int m, int n) +{ + if ((m > n) || (std::fabs(m - n) < MAX_FLOAT_ERR)) { + return testing::AssertionSuccess(); + } + + return testing::AssertionFailure() << "Expected: " << m_expr << " <= " << n_expr << std::endl + << " Actual: " << m << " vs " << n << std::endl + << "Using max floating point error of: " << MAX_FLOAT_ERR << std::endl; +} + +static std::random_device g_rd = std::random_device(); // random number sampler +static uint32_t g_rand_seed = g_rd(); // seed used for random number generation + +class TestMockAisSim : public ::testing::Test +{ +protected: + MockAis sim_; + TestMockAisSim() : sim_(g_rand_seed, NUM_SHIPS, POLARIS_START_POS) + { + SCOPED_TRACE("Seed: " + std::to_string(g_rand_seed)); + } + ~TestMockAisSim(){}; +}; + +/** + * @brief Verify that an Ais ship is within acceptable distance from Polaris + * + * @param ais_ship_lat_lon ais ship position + * @param polaris_lat_lon polaris position + */ +void checkAisShipInBounds(Vec2DFloat ais_ship_lat_lon, Vec2DFloat polaris_lat_lon) +{ + Vec2DFloat displacement = ais_ship_lat_lon - polaris_lat_lon; + float distance = qvm::mag(displacement); + EXPECT_PRED_FORMAT2(AssertFloatLE, distance, MAX_AIS_SHIP_DIST); + EXPECT_PRED_FORMAT2(AssertFloatGE, distance, MIN_AIS_SHIP_DIST); +} + +/** + * @brief Verify that ais ships operate within specified constraints + * + * @param updated_ship ais ship instance + * @param past_ship same ais ship instance but saved from the previous tick + */ +void checkAisShipTickUpdateLimits(AisShip updated_ship, AisShip past_ship) +{ + EXPECT_LE(updated_ship.speed_, SPEED_UBND); + EXPECT_GE(updated_ship.speed_, SPEED_LBND); + EXPECT_LT(updated_ship.heading_, HEADING_UBND); + EXPECT_GE(updated_ship.heading_, HEADING_LBND); + + if (std::abs(updated_ship.speed_ - past_ship.speed_) > MAX_SPEED_CHANGE) { + if (updated_ship.speed_ < past_ship.speed_) { + EXPECT_LE(updated_ship.speed_ + SPEED_UBND - past_ship.speed_, MAX_SPEED_CHANGE); + } else { + EXPECT_LE(past_ship.speed_ + SPEED_UBND - updated_ship.speed_, MAX_SPEED_CHANGE); + } + } else { + // Passes check + } + if (std::abs(updated_ship.heading_ - past_ship.heading_) > MAX_HEADING_CHANGE) { + if (updated_ship.heading_ < past_ship.heading_) { + EXPECT_LE(updated_ship.heading_ + HEADING_UBND - past_ship.heading_, MAX_HEADING_CHANGE); + } else { + EXPECT_LE(past_ship.heading_ + HEADING_UBND - updated_ship.heading_, MAX_HEADING_CHANGE); + } + } else { + // Passes check + } + + EXPECT_LE(updated_ship.rot_, ROT_UBND); + EXPECT_GE(updated_ship.rot_, ROT_LBND); +} + +/** + * @brief Test basic operation when Polaris is not moving + * + */ +TEST_F(TestMockAisSim, TestBasic) +{ + std::vector curr_ships = sim_.ships(); + for (uint32_t i = 0; i < NUM_TEST_CYCLES; i++) { + sim_.tick(); + std::vector updated_ships = sim_.ships(); + for (uint32_t i = 0; i < NUM_SHIPS; i++) { + EXPECT_EQ(updated_ships[i].id_, curr_ships[i].id_); + checkAisShipTickUpdateLimits(updated_ships[i], curr_ships[i]); + checkAisShipInBounds(updated_ships[i].lat_lon_, POLARIS_START_POS); + } + curr_ships = updated_ships; + } +} + +/** + * @brief Test operation when Polaris is moving + * + */ +TEST_F(TestMockAisSim, TestMovingPolaris) +{ + Vec2DFloat polaris_lat_lon = POLARIS_START_POS; + std::vector curr_ships = sim_.ships(); + for (uint32_t i = 0; i < NUM_TEST_CYCLES; i++) { + sim_.tick(); + std::vector updated_ships = sim_.ships(); + for (uint32_t i = 0; i < NUM_SHIPS; i++) { + EXPECT_EQ(updated_ships[i].id_, curr_ships[i].id_); + checkAisShipTickUpdateLimits(updated_ships[i], curr_ships[i]); + checkAisShipInBounds(updated_ships[i].lat_lon_, polaris_lat_lon); + } + curr_ships = updated_ships; + polaris_lat_lon[0] += 2 * MIN_AIS_SHIP_DIST; + polaris_lat_lon[1] += 2 * MIN_AIS_SHIP_DIST; + sim_.updatePolarisPos(polaris_lat_lon); + } +} diff --git a/projects/remote_transceiver/test/test_remote_transceiver.cpp b/projects/remote_transceiver/test/test_remote_transceiver.cpp index 8979b00..e8f1375 100644 --- a/projects/remote_transceiver/test/test_remote_transceiver.cpp +++ b/projects/remote_transceiver/test/test_remote_transceiver.cpp @@ -279,8 +279,8 @@ void genRandAisData(Sensors::Ais * ais_ship) std::uniform_real_distribution speed_dist(SPEED_LBND, SPEED_UBND); std::uniform_real_distribution heading_dist(HEADING_LBND, HEADING_UBND); std::uniform_real_distribution rot_dist(ROT_LBND, ROT_UBND); - std::uniform_real_distribution width_dist(DIMENSION_LBND, DIMENSION_UBND); - std::uniform_real_distribution length_dist(DIMENSION_LBND, DIMENSION_UBND); + std::uniform_real_distribution width_dist(SHIP_DIMENSION_LBND, SHIP_DIMENSION_UBND); + std::uniform_real_distribution length_dist(SHIP_DIMENSION_LBND, SHIP_DIMENSION_UBND); ais_ship->set_id(id_dist(g_mt)); ais_ship->set_latitude(lat_dist(g_mt)); @@ -313,8 +313,8 @@ void genRandGenericSensorData(Sensors::Generic * generic_sensor) */ void genRandBatteriesData(Sensors::Battery * battery) { - std::uniform_real_distribution voltage_battery(VOLT_LBND, VOLT_UBND); - std::uniform_real_distribution current_battery(CURRENT_LBND, CURRENT_UBND); + std::uniform_real_distribution voltage_battery(BATT_VOLT_LBND, BATT_VOLT_UBND); + std::uniform_real_distribution current_battery(BATT_CURR_LBND, BATT_CURR_UBND); battery->set_voltage(voltage_battery(g_mt)); battery->set_current(current_battery(g_mt)); @@ -328,7 +328,7 @@ void genRandBatteriesData(Sensors::Battery * battery) void genRandWindData(Sensors::Wind * wind_data) { std::uniform_real_distribution speed_wind(SPEED_LBND, SPEED_UBND); - std::uniform_int_distribution direction_wind(DIRECTION_LBND, DIRECTION_UBND); + std::uniform_int_distribution direction_wind(WIND_DIRECTION_LBND, WIND_DIRECTION_UBND); wind_data->set_speed(speed_wind(g_mt)); wind_data->set_direction(direction_wind(g_mt));