diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt index c83f8364..1c2ced81 100644 --- a/subt_ign/CMakeLists.txt +++ b/subt_ign/CMakeLists.txt @@ -52,13 +52,8 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${PROJECT_BINARY_DIR}/include - ${CATKIN_DEVEL_PREFIX}/include ) -message(STATUS ${LIBFCL_INCLUDE_DIRS}) - -link_directories() - catkin_package( CATKIN_DEPENDS subt_rf_interface @@ -66,7 +61,6 @@ catkin_package( subt_communication_broker INCLUDE_DIRS include - ${CATKIN_DEVEL_PREFIX}/include LIBRARIES SubtProtobuf CFG_EXTRAS @@ -99,350 +93,120 @@ catkin_add_env_hooks(ign SHELLS bash catkin_add_env_hooks(ign SHELLS zsh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks) -# Create the libGameLogicPlugin.so library. -set(game_logic_plugin_name GameLogicPlugin) -add_library(${game_logic_plugin_name} SHARED src/GameLogicPlugin.cc) -target_include_directories(${game_logic_plugin_name} - PRIVATE - ${PROTOBUF_INCLUDE_DIRS} - ${CATKIN_DEVEL_PREFIX}/include) -target_link_libraries(${game_logic_plugin_name} - PRIVATE - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-common3::ignition-common3 - ignition-launch3::ignition-launch3 - ignition-math6::ignition-math6 - ignition-msgs6::ignition-msgs6 - ignition-plugin1::loader - ignition-transport9::ignition-transport9 - ${catkin_LIBRARIES} - ${protobuf_lib_name} - ${YAML_CPP_LIBRARIES} -) -install(TARGETS ${game_logic_plugin_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -set(artifact_validator_name ArtifactValidator) -add_library(${artifact_validator_name} SHARED src/ArtifactValidator.cc) -target_link_libraries(${artifact_validator_name} - PRIVATE - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-common3::ignition-common3 - ignition-launch3::ignition-launch3 - ignition-math6::ignition-math6 - ignition-msgs6::ignition-msgs6 - ignition-plugin1::loader - ignition-transport9::ignition-transport9 - ${catkin_LIBRARIES} - ${protobuf_lib_name} -) -install(TARGETS ${artifact_validator_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -set(connection_validator_name ConnectionValidator) -add_library(${connection_validator_name} SHARED - src/ConnectionValidator.cc - src/ConnectionValidatorPrivate.cc - src/ConnectionHelper.cc - src/SimpleDOTParser.cc) -target_link_libraries(${connection_validator_name} - PRIVATE - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-common3::ignition-common3 - ignition-launch3::ignition-launch3 - ignition-math6::ignition-math6 - ignition-msgs6::ignition-msgs6 - ignition-plugin1::loader - ignition-transport9::ignition-transport9 - ${catkin_LIBRARIES} - ${protobuf_lib_name} -) -install(TARGETS ${connection_validator_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -add_executable(validate_connections - src/validate_connections.cc - src/ConnectionValidator.cc +add_library(SubtCommon + SHARED + src/Common.cc src/ConnectionValidatorPrivate.cc src/ConnectionHelper.cc - src/SimpleDOTParser.cc) -target_link_libraries(validate_connections - PRIVATE - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-common3::ignition-common3 - ignition-launch3::ignition-launch3 - ignition-math6::ignition-math6 - ignition-msgs6::ignition-msgs6 - ignition-plugin1::loader - ignition-transport9::ignition-transport9 - ${catkin_LIBRARIES} - ${protobuf_lib_name} -) -install(TARGETS validate_connections - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -add_executable(path_tracer - src/path_tracer.cc) -target_link_libraries(path_tracer - PRIVATE - ignition-gazebo${IGN_GAZEBO_VER}::core - ${catkin_LIBRARIES} - ${YAML_CPP_LIBRARIES} -) -install(TARGETS path_tracer - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -add_library(Visibility - STATIC src/ign_to_fcl.cc + src/SdfParser.cc src/SimpleDOTParser.cc src/VisibilityRfModel.cc src/VisibilityTable.cc ) -target_include_directories(Visibility + +target_include_directories(SubtCommon PUBLIC - ${catkin_INCLUDE_DIRS} - ${LIBFCL_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/src ) -target_link_libraries( - Visibility + +target_link_libraries(SubtCommon PUBLIC ignition-gazebo${IGN_GAZEBO_VER}::core ignition-common3::ignition-common3 ignition-common3::graphics - ${LIBFCL_LIBRARIES} + ignition-transport9::ignition-transport9 + ignition-transport9::ignition-transport9-log + ignition-launch3::ignition-launch3 + ignition-fuel_tools5::ignition-fuel_tools5 ${catkin_LIBRARIES} + ${LIBFCL_LIBRARIES} + ${YAML_CPP_LIBRARIES} ) -add_executable(validate_visibility_table - src/validate_visibility_table.cc) -target_link_libraries(validate_visibility_table - PRIVATE - Visibility +list(APPEND SUBT_IGN_PLUGINS + ArtifactValidator + BaseStationPlugin + CommsBrokerPlugin + ConnectionValidator + ControllerPlugin + GameLogicPlugin + GasEmitterDetectorPlugin ) -install(TARGETS validate_visibility_table - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) -# Create the libCommsBrokerPlugin.so library. -set(comms_broker_plugin_name CommsBrokerPlugin) -add_library(${comms_broker_plugin_name} - src/CommsBrokerPlugin.cc -) -target_include_directories(${comms_broker_plugin_name} - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) -target_link_libraries(${comms_broker_plugin_name} - PUBLIC - ignition-common3::ignition-common3 - ignition-math6::ignition-math6 - ignition-msgs6::ignition-msgs6 - ignition-plugin1::loader - ignition-launch3::ignition-launch3 - ignition-transport9::ignition-transport9 - sdformat10::sdformat10 - ${protobuf_lib_name} - Visibility -) -install(TARGETS ${comms_broker_plugin_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) +foreach(PLUGIN ${SUBT_IGN_PLUGINS}) + add_library(${PLUGIN} SHARED src/${PLUGIN}.cc) + target_link_libraries(${PLUGIN} PUBLIC SubtCommon SubtProtobuf) +endforeach() -# Create the libVisibilityPlugin.so library. -set(visibility_plugin_name VisibilityPlugin) -add_library(${visibility_plugin_name} SHARED - src/VisibilityPlugin.cc -) -target_link_libraries(${visibility_plugin_name} - PRIVATE - ignition-common3::ignition-common3 - ignition-math6::ignition-math6 - ignition-msgs6::ignition-msgs6 - ignition-plugin1::loader - ignition-launch3::ignition-launch3 - ignition-transport9::ignition-transport9 - sdformat10::sdformat10 - ${protobuf_lib_name} - Visibility -) -install(TARGETS ${visibility_plugin_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -# Create the libBaseStationPlugin.so library. -set(base_station_plugin_name BaseStationPlugin) -add_library(${base_station_plugin_name} src/BaseStationPlugin.cc) -target_include_directories(${base_station_plugin_name} - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) - target_link_libraries(${base_station_plugin_name} - ignition-common3::ignition-common3 - ignition-plugin1::loader - ignition-launch3::ignition-launch3 - ignition-transport9::ignition-transport9 - sdformat10::sdformat10 - ${catkin_LIBRARIES} - ${protobuf_lib_name} - ) - install(TARGETS ${base_station_plugin_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -# Create the libControllerPlugin.so library. -set(controller_plugin_name ControllerPlugin) -add_library(${controller_plugin_name} src/ControllerPlugin.cc) -target_include_directories(${controller_plugin_name} - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) - target_link_libraries(${controller_plugin_name} - ignition-common3::ignition-common3 - ignition-plugin1::loader - ignition-launch3::ignition-launch3 - ignition-transport9::ignition-transport9 - sdformat10::sdformat10 - ${catkin_LIBRARIES} - ${protobuf_lib_name} - ) - install(TARGETS ${controller_plugin_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -# Create the libGasEmitterDetectorPlugin.so library. -set(gas_emitter_detector_plugin_name GasEmitterDetectorPlugin) -add_library(${gas_emitter_detector_plugin_name} SHARED src/GasEmitterDetectorPlugin.cc) -target_include_directories(${gas_emitter_detector_plugin_name} - PRIVATE - ${PROTOBUF_INCLUDE_DIRS} - ${CATKIN_DEVEL_PREFIX}/include) -target_link_libraries(${gas_emitter_detector_plugin_name} - PRIVATE - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-common3::ignition-common3 - ignition-launch3::ignition-launch3 - ignition-math6::ignition-math6 - ignition-msgs6::ignition-msgs6 - ignition-plugin1::loader - ignition-transport9::ignition-transport9 - ${catkin_LIBRARIES} - ${protobuf_lib_name} -) -install(TARGETS ${gas_emitter_detector_plugin_name} +install( + TARGETS SubtProtobuf SubtCommon ${SUBT_IGN_PLUGINS} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) -# Create log_checker executable. -add_executable(log_checker src/LogChecker.cc) -target_link_libraries(log_checker - ignition-transport9::ignition-transport9 - ignition-transport9::ignition-transport9-log) -install(TARGETS log_checker - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +add_executable(validate_connections src/apps/validate_connections.cc) +target_link_libraries(validate_connections ConnectionValidator) -add_executable(dot_generator src/dot_generator.cc src/ConnectionHelper.cc src/SdfParser.cc) -target_link_libraries(dot_generator - ignition-math6::ignition-math6 - ignition-common3::ignition-common3 - sdformat10::sdformat10) -target_include_directories(dot_generator - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) -install(TARGETS dot_generator - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -add_executable(level_generator src/level_generator.cc src/ConnectionHelper.cc src/SdfParser.cc) -target_link_libraries(level_generator - ignition-math6::ignition-math6 - ignition-common3::ignition-common3 - sdformat10::sdformat10) -target_include_directories(level_generator - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) -install(TARGETS level_generator - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -add_executable(world_generator_cave src/world_generator_cave.cc src/ConnectionHelper.cc) -target_link_libraries(world_generator_cave - ignition-math6::ignition-math6 - ignition-common3::requested - ignition-fuel_tools5::ignition-fuel_tools5 - sdformat10::sdformat10) -target_include_directories(world_generator_cave - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) -install(TARGETS world_generator_cave - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +add_executable(path_tracer src/apps/path_tracer.cc) +target_link_libraries(path_tracer SubtCommon) + +add_executable(validate_visibility_table src/apps/validate_visibility_table.cc) +target_link_libraries(validate_visibility_table SubtCommon) +# Create log_checker executable. +add_executable(log_checker src/apps/LogChecker.cc) +target_link_libraries(log_checker SubtCommon) + +add_executable(dot_generator src/apps/dot_generator.cc) +target_link_libraries(dot_generator SubtCommon) + +add_executable(level_generator src/apps/level_generator.cc) +target_link_libraries(level_generator SubtCommon) + +add_executable(world_generator_cave src/apps/world_generator_cave.cc) +target_link_libraries(world_generator_cave SubtCommon) + +install( + TARGETS + validate_connections + path_tracer + validate_visibility_table + log_checker + dot_generator + level_generator + world_generator_cave + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ########### ## Tests ## ########### find_package(rostest REQUIRED) - -# test_score was ported from gazebo9, but doesn't work as rostest -# since it needs ign launch. so just compile it, but don't -# declare it as a rostest -#add_rostest_gtest(test_score test/score.test test/score.cc) -if(GTEST_FOUND) - add_executable(test_score EXCLUDE_FROM_ALL test/score.cc) - target_link_libraries(test_score ${GTEST_LIBRARIES}) - if(TARGET tests) - add_dependencies(tests test_score) - endif() -endif() -target_include_directories(test_score - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) -target_link_libraries(test_score - ${protobuf_lib_name} - ${catkin_LIBRARIES} -) -add_dependencies(test_score - ${game_logic_plugin_name} -) - -#add_rostest_gtest(max_test_score test/max_score.test test/max_score.cc) -if(GTEST_FOUND) - add_executable(max_test_score EXCLUDE_FROM_ALL test/max_score.cc) - target_link_libraries(max_test_score ${GTEST_LIBRARIES}) - if(TARGET tests) - add_dependencies(tests max_test_score) - endif() -endif() -target_include_directories(max_test_score - PRIVATE ${CATKIN_DEVEL_PREFIX}/include) -target_link_libraries(max_test_score - ${protobuf_lib_name} - ${catkin_LIBRARIES} -) -add_dependencies(max_test_score - ${game_logic_plugin_name} -) - if (CATKIN_ENABLE_TESTING) configure_file(test/test_config.hh.in ${CMAKE_CURRENT_BINARY_DIR}/test_config.hh) - catkin_add_gtest(gas_emitter_detector test/gas_emitter_detector.cc) - - target_include_directories( - gas_emitter_detector - PRIVATE - ${CMAKE_CURRENT_BINARY_DIR} - ) - - target_link_libraries(gas_emitter_detector - ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-common3::ignition-common3 - ) + # Score Test + add_executable(test_score test/score.cc) + target_link_libraries(test_score SubtCommon SubtProtobuf ${GTEST_LIBRARIES}) + add_dependencies(test_score GameLogicPlugin) + + # Max Score Test + add_executable(max_test_score test/max_score.cc) + target_link_libraries(max_test_score SubtCommon SubtProtobuf ${GTEST_LIBRARIES}) + add_dependencies(max_test_score GameLogicPlugin) + + # GasEmitterDetector Test + catkin_add_gtest(gas_emitter_detector_TEST test/gas_emitter_detector.cc) + target_include_directories(gas_emitter_detector_TEST PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) + target_link_libraries(gas_emitter_detector_TEST SubtCommon) + + # Common Test + catkin_add_gtest(common_TEST test/Common_TEST.cc) + target_include_directories(common_TEST PRIVATE ${CMAKE_CURRENT_BINARY_DIR}) + target_link_libraries(common_TEST SubtCommon) endif() diff --git a/subt_ign/include/subt_ign/Common.hh b/subt_ign/include/subt_ign/Common.hh new file mode 100644 index 00000000..d82fa0c4 --- /dev/null +++ b/subt_ign/include/subt_ign/Common.hh @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef SUBT_IGN_COMMON_HH_ +#define SUBT_IGN_COMMON_HH_ + +#include +#include + +#include + +namespace subt +{ + /// \brief Mapping between artifact model names and types. + const std::array< + const std::pair, 14> kArtifactNames + { + { + {"backpack", ArtifactType::TYPE_BACKPACK}, + {"drill", ArtifactType::TYPE_DRILL}, + {"duct", ArtifactType::TYPE_DUCT}, + {"electrical_box", ArtifactType::TYPE_ELECTRICAL_BOX}, + {"extinguisher", ArtifactType::TYPE_EXTINGUISHER}, + {"phone", ArtifactType::TYPE_PHONE}, + {"radio", ArtifactType::TYPE_RADIO}, + {"rescue_randy", ArtifactType::TYPE_RESCUE_RANDY}, + {"toolbox", ArtifactType::TYPE_TOOLBOX}, + {"valve", ArtifactType::TYPE_VALVE}, + {"vent", ArtifactType::TYPE_VENT}, + {"gas", ArtifactType::TYPE_GAS}, + {"helmet", ArtifactType::TYPE_HELMET}, + {"rope", ArtifactType::TYPE_ROPE} + } + }; + + /// \brief Mapping between enum types and strings. + const std::array< + const std::pair, 14> kArtifactTypes + { + { + {ArtifactType::TYPE_BACKPACK , "TYPE_BACKPACK"}, + {ArtifactType::TYPE_DRILL , "TYPE_DRILL"}, + {ArtifactType::TYPE_DUCT , "TYPE_DUCT"}, + {ArtifactType::TYPE_ELECTRICAL_BOX, "TYPE_ELECTRICAL_BOX"}, + {ArtifactType::TYPE_EXTINGUISHER , "TYPE_EXTINGUISHER"}, + {ArtifactType::TYPE_PHONE , "TYPE_PHONE"}, + {ArtifactType::TYPE_RADIO , "TYPE_RADIO"}, + {ArtifactType::TYPE_RESCUE_RANDY , "TYPE_RESCUE_RANDY"}, + {ArtifactType::TYPE_TOOLBOX , "TYPE_TOOLBOX"}, + {ArtifactType::TYPE_VALVE , "TYPE_VALVE"}, + {ArtifactType::TYPE_VENT , "TYPE_VENT"}, + {ArtifactType::TYPE_GAS , "TYPE_GAS"}, + {ArtifactType::TYPE_HELMET , "TYPE_HELMET"}, + {ArtifactType::TYPE_ROPE , "TYPE_ROPE"} + } + }; + + /// \brief Create an ArtifactType from a string. + /// \param[in] _name The artifact in string format. + /// \param[out] _type The artifact type. + /// \return True when the conversion succeed or false otherwise. + bool ArtifactFromString(const std::string &_name, + subt::ArtifactType &_type); + + /// \brief Create an ArtifactType from a string. + /// \param[in] _name The artifact in string format. + /// \param[out] _type The artifact type. + /// \return True when the conversion succeed or false otherwise. + bool ArtifactFromPartialString(const std::string &_name, + subt::ArtifactType &_type); + + /// \brief Create an ArtifactType from an integer. + // + /// \param[in] _typeInt The artifact in int format. + /// \param[out] _type The artifact type. + /// \return True when the conversion succeed or false otherwise. + bool ArtifactFromInt(const uint32_t &_typeInt, + subt::ArtifactType &_type); + + /// \brief Create a string from ArtifactType. + // + /// \param[in] _type The artifact type. + /// \param[out] _strType The artifact string. + /// \return True when the conversion succeed or false otherwise. + bool StringFromArtifact(const subt::ArtifactType &_type, + std::string &_strType); + + /// \brief Retrieve a fully-qualifed path based on name + /// + /// This accounts for the SubT world file directory + /// structure. + // + /// \param[in] _worldName The name of the world + /// For example: cave_circuit_practice_02 + /// \param[out] _worldPath The world path without extension. + /// \return True when the path is expanded successfully. + bool FullWorldPath(const std::string &_worldName, + std::string &_worldPath); + + /// \brief Structure to represent an artifact as parsed + struct Artifact + { + /// \brief Artifact name + std::string name; + + /// \brief Artifact type + subt::ArtifactType type; + + /// \brief Artifact type as a string + std::string typeStr; + + /// \brief Artifact pose + ignition::math::Pose3d pose; + + /// \brief Return a string representation of the artifact + std::string String() const + { + std::stringstream ss; + ss << "pose << ")" + << ">"; + return ss.str(); + } + }; +} +#endif diff --git a/subt_ign/include/subt_ign/CommonTypes.hh b/subt_ign/include/subt_ign/CommonTypes.hh index 8883a065..773ec6aa 100644 --- a/subt_ign/include/subt_ign/CommonTypes.hh +++ b/subt_ign/include/subt_ign/CommonTypes.hh @@ -79,7 +79,8 @@ namespace subt TYPE_VENT, TYPE_GAS, TYPE_HELMET, - TYPE_ROPE + TYPE_ROPE, + Count }; /// \def TeamMemberPtr diff --git a/subt_ign/src/ArtifactValidator.cc b/subt_ign/src/ArtifactValidator.cc index 73ecc8d8..4ef08a12 100644 --- a/subt_ign/src/ArtifactValidator.cc +++ b/subt_ign/src/ArtifactValidator.cc @@ -63,7 +63,7 @@ ign service -s /artifact/move_to \ #include #include -#include "subt_ign/CommonTypes.hh" +#include "subt_ign/Common.hh" IGNITION_ADD_PLUGIN( subt::ArtifactValidator, @@ -77,68 +77,9 @@ using namespace gazebo; using namespace systems; using namespace subt; -/// \brief Simple structure to represent an artifact -struct Artifact -{ - /// \brief Artifact name - std::string name; - - /// \brief Artifact type - subt::ArtifactType type; - - /// \brief Artifact type as a string - std::string typeStr; - - /// \brief Artifact pose - ignition::math::Pose3d pose; - - /// \brief Return a string representation of the artifact - std::string String() const - { - std::stringstream ss; - ss << "pose << ")" - << ">"; - return ss.str(); - } -}; - /// \brief Private data for the artifact validator. class subt::ArtifactValidatorPrivate { - /// \brief Map of artifact types to string representations. - public: const std::array< - const std::pair, 14> kArtifactTypes - { - { - {subt::ArtifactType::TYPE_BACKPACK , "TYPE_BACKPACK"}, - {subt::ArtifactType::TYPE_DRILL , "TYPE_DRILL"}, - {subt::ArtifactType::TYPE_DUCT , "TYPE_DUCT"}, - {subt::ArtifactType::TYPE_ELECTRICAL_BOX, "TYPE_ELECTRICAL_BOX"}, - {subt::ArtifactType::TYPE_EXTINGUISHER , "TYPE_EXTINGUISHER"}, - {subt::ArtifactType::TYPE_PHONE , "TYPE_PHONE"}, - {subt::ArtifactType::TYPE_RADIO , "TYPE_RADIO"}, - {subt::ArtifactType::TYPE_RESCUE_RANDY , "TYPE_RESCUE_RANDY"}, - {subt::ArtifactType::TYPE_TOOLBOX , "TYPE_TOOLBOX"}, - {subt::ArtifactType::TYPE_VALVE , "TYPE_VALVE"}, - {subt::ArtifactType::TYPE_VENT , "TYPE_VENT"}, - {subt::ArtifactType::TYPE_GAS , "TYPE_GAS"}, - {subt::ArtifactType::TYPE_HELMET , "TYPE_HELMET"}, - {subt::ArtifactType::TYPE_ROPE , "TYPE_ROPE"} - } - }; - - /// \brief Get the artifact enumeration from the string value. - /// \param[in] _name - string representation of artifact - /// \param[out] _type - enumeration representation of artifact - public: bool ArtifactFromString( - const std::string &_name, ArtifactType &_type); - - public: bool StringFromArtifact(const ArtifactType &_type, - std::string &_typeStr); - /// \brief Parse artifacts from the SDF root. public: void ParseArtifacts(); @@ -252,65 +193,9 @@ bool ArtifactValidatorPrivate::OnPrev(const ignition::msgs::StringMsg& /*_req*/, return ret; } -///////////////////////////////////////////////// -bool ArtifactValidatorPrivate::ArtifactFromString(const std::string &_name, - ArtifactType &_type) -{ - auto pos = std::find_if( - std::begin(this->kArtifactTypes), - std::end(this->kArtifactTypes), - [&_name](const typename std::pair &_pair) - { - return (std::get<1>(_pair) == _name); - }); - - if (pos == std::end(this->kArtifactTypes)) - return false; - - _type = std::get<0>(*pos); - return true; -} - -///////////////////////////////////////////////// -bool ArtifactValidatorPrivate::StringFromArtifact(const ArtifactType &_type, - std::string &_typeStr) -{ - auto pos = std::find_if( - std::begin(this->kArtifactTypes), - std::end(this->kArtifactTypes), - [&_type](const typename std::pair &_pair) - { - return (std::get<0>(_pair) == _type); - }); - - if (pos == std::end(this->kArtifactTypes)) - return false; - - _typeStr = std::get<1>(*pos); - return true; -} - - ///////////////////////////////////////////////// void ArtifactValidatorPrivate::ParseArtifacts() { - std::map sdfToType = { - { "backpack", subt::ArtifactType::TYPE_BACKPACK }, - { "drill", subt::ArtifactType::TYPE_DRILL}, - { "duct", subt::ArtifactType::TYPE_DUCT}, - { "electrical_box", subt::ArtifactType::TYPE_ELECTRICAL_BOX}, - { "extinguisher", subt::ArtifactType::TYPE_EXTINGUISHER}, - { "phone", subt::ArtifactType::TYPE_PHONE}, - { "radio", subt::ArtifactType::TYPE_RADIO}, - { "rescue_randy", subt::ArtifactType::TYPE_RESCUE_RANDY }, - { "toolbox", subt::ArtifactType::TYPE_TOOLBOX }, - { "valve", subt::ArtifactType::TYPE_VALVE}, - { "vent", subt::ArtifactType::TYPE_VENT}, - { "gas", subt::ArtifactType::TYPE_GAS}, - { "helmet", subt::ArtifactType::TYPE_HELMET}, - { "rope", subt::ArtifactType::TYPE_ROPE} - }; - // Assuming 1 world per SDF, which is true for SubT. auto world = this->sdfRoot.WorldByIndex(0); @@ -328,7 +213,7 @@ void ArtifactValidatorPrivate::ParseArtifacts() artifacts[name] = newArtifact; } - for (auto [str, type] : sdfToType) + for (auto [str, type] : kArtifactNames) { if (name.find(str) != std::string::npos) { @@ -376,33 +261,14 @@ void ArtifactValidator::Configure(const ignition::gazebo::Entity & /*_entity*/, } this->dataPtr->worldName = worldName; - auto vals = ignition::common::Split(worldName, '_'); - auto worldNum = vals.back(); - if (worldName.find("tunnel_circuit_") != std::string::npos && - worldName.find("practice") == std::string::npos) - { - worldName = "tunnel_circuit/" + worldNum + "/" + worldName +".sdf"; - } - else if (worldName.find("urban_circuit_") != std::string::npos && - worldName.find("practice") == std::string::npos) - { - worldName = "urban_circuit/" + worldNum + "/" + worldName + ".sdf"; - } - else if (worldName.find("cave_circuit_") != std::string::npos && - worldName.find("practice") == std::string::npos) - { - worldName = "cave_circuit/" + worldNum + "/" + worldName + ".sdf"; - } - else - { - worldName = worldName + ".sdf"; - } + std::string fullPath; + subt::FullWorldPath(worldName, fullPath); common::SystemPaths systemPaths; systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH"); systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); - std::string filePath = systemPaths.FindFile(worldName); + std::string filePath = systemPaths.FindFile(fullPath); ignmsg << "Loading SDF world file[" << filePath << "].\n"; auto errors = this->dataPtr->sdfRoot.Load(filePath); diff --git a/subt_ign/src/Common.cc b/subt_ign/src/Common.cc new file mode 100644 index 00000000..9ea4368e --- /dev/null +++ b/subt_ign/src/Common.cc @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +namespace subt +{ + +///////////////////////////////////////////////// +bool ArtifactFromInt(const uint32_t &_typeInt, ArtifactType &_type) +{ + if (_typeInt > (kArtifactTypes.size() - 1)) + return false; + + _type = static_cast(_typeInt); + return true; +} + +///////////////////////////////////////////////// +bool ArtifactFromString(const std::string &_name, + ArtifactType &_type) +{ + auto pos = std::find_if( + std::begin(kArtifactTypes), + std::end(kArtifactTypes), + [&_name](const typename std::pair &_pair) + { + return (std::get<1>(_pair) == _name); + }); + + if (pos == std::end(kArtifactTypes)) + return false; + + _type = std::get<0>(*pos); + return true; +} + +///////////////////////////////////////////////// +bool ArtifactFromPartialString(const std::string &_name, + ArtifactType &_type) +{ + auto pos = std::find_if( + std::begin(kArtifactNames), + std::end(kArtifactNames), + [&_name](const auto &_pair) + { + return _name.find(std::get<0>(_pair)) != std::string::npos; + }); + + if (pos == std::end(kArtifactNames)) + return false; + + _type = std::get<1>(*pos); + return true; +} + +///////////////////////////////////////////////// +bool StringFromArtifact(const ArtifactType &_type, + std::string &_strType) +{ + auto pos = std::find_if( + std::begin(kArtifactTypes), + std::end(kArtifactTypes), + [&_type](const typename std::pair &_pair) + { + return (std::get<0>(_pair) == _type); + }); + + if (pos == std::end(kArtifactTypes)) + return false; + + _strType = std::get<1>(*pos); + return true; +} + +///////////////////////////////////////////////// +bool FullWorldPath(const std::string &_worldName, + std::string &_worldPath) +{ + _worldPath = ""; + if (_worldName.empty()) + { + return false; + } + + std::string worldsDirectory; + + const std::string tunnelPrefix = "tunnel_circuit_"; + const std::string urbanPrefix = "urban_circuit_"; + const std::string cavePrefix = "cave_circuit_"; + if (0 == _worldName.compare(0, tunnelPrefix.size(), tunnelPrefix)) + { + std::string suffix = _worldName.substr(tunnelPrefix.size()); + // don't use a subfolder for practice worlds + if (0 != suffix.compare(0, 9, "practice_")) + { + worldsDirectory = ignition::common::joinPaths(worldsDirectory, + "tunnel_circuit", suffix); + } + } + else if (_worldName.find(urbanPrefix) != std::string::npos) + { + std::string suffix = _worldName.substr(urbanPrefix.size()); + // don't use a subfolder for practice worlds + if (0 != suffix.compare(0, 9, "practice_")) + { + worldsDirectory = ignition::common::joinPaths(worldsDirectory, + "urban_circuit", suffix); + } + } + else if (_worldName.find(cavePrefix) != std::string::npos) + { + std::string suffix = _worldName.substr(cavePrefix.size()); + // don't use a subfolder for practice worlds + if (0 != suffix.compare(0, 9, "practice_")) + { + worldsDirectory = ignition::common::joinPaths(worldsDirectory, + "cave_circuit", suffix); + } + } + else if (_worldName.find("simple") == std::string::npos && + _worldName.find("_qual") == std::string::npos) + { + return false; + } + + _worldPath = ignition::common::joinPaths(worldsDirectory, _worldName); + return true; +} + +} // namespace subt diff --git a/subt_ign/src/ConnectionValidatorPrivate.cc b/subt_ign/src/ConnectionValidatorPrivate.cc index ded334af..bbc4393d 100644 --- a/subt_ign/src/ConnectionValidatorPrivate.cc +++ b/subt_ign/src/ConnectionValidatorPrivate.cc @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -38,52 +39,10 @@ bool ConnectionValidatorPrivate::Load(const std::string &_worldName) { std::string worldsDirectory = SUBT_INSTALL_WORLD_DIR; - // Modifications for the tunnel circuit. - const std::string tunnelPrefix = "tunnel_circuit_"; - const std::string urbanPrefix = "urban_circuit_"; - const std::string cavePrefix = "cave_circuit_"; - if (0 == _worldName.compare(0, tunnelPrefix.size(), tunnelPrefix)) - { - std::string suffix = _worldName.substr(tunnelPrefix.size()); - // don't use a subfolder for practice worlds - if (0 != suffix.compare(0, 9, "practice_")) - { - worldsDirectory = ignition::common::joinPaths(worldsDirectory, - "tunnel_circuit", suffix); - } - } - else if (0 == _worldName.compare(0, urbanPrefix.size(), urbanPrefix)) - { - std::string suffix = _worldName.substr(urbanPrefix.size()); - // don't use a subfolder for practice worlds - if (0 != suffix.compare(0, 9, "practice_")) - { - worldsDirectory = ignition::common::joinPaths(worldsDirectory, - "urban_circuit", suffix); - } - } - else if (0 == _worldName.compare(0, cavePrefix.size(), cavePrefix)) - { - std::string suffix = _worldName.substr(cavePrefix.size()); - // don't use a subfolder for practice worlds - if (0 != suffix.compare(0, 9, "practice_")) - { - worldsDirectory = ignition::common::joinPaths(worldsDirectory, - "cave_circuit", suffix); - } - } - else if (_worldName.find("simple") == std::string::npos && - _worldName.find("_qual") == std::string::npos) - { - ignwarn << "Unable to determine circuit number from[" - << _worldName << "].\n"; - } - - std::string worldPath = common::joinPaths( - worldsDirectory, _worldName + ".sdf"); - - std::string graphPath = common::joinPaths( - worldsDirectory, _worldName + ".dot"); + std::string fullPath; + subt::FullWorldPath(_worldName, fullPath); + std::string worldPath = fullPath + ".sdf"; + std::string graphPath = fullPath + ".dot"; auto ret = LoadSdf(worldPath) && LoadDot(graphPath); if (ret) diff --git a/subt_ign/src/GameLogicPlugin.cc b/subt_ign/src/GameLogicPlugin.cc index 32d6fe38..b9dea03b 100644 --- a/subt_ign/src/GameLogicPlugin.cc +++ b/subt_ign/src/GameLogicPlugin.cc @@ -67,7 +67,7 @@ #include "subt_ros/RobotEvent.h" #include "subt_ros/RunStatistics.h" #include "subt_ros/RunStatus.h" -#include "subt_ign/CommonTypes.hh" +#include "subt_ign/Common.hh" #include "subt_ign/GameLogicPlugin.hh" #include "subt_ign/protobuf/artifact.pb.h" #include "subt_ign/RobotPlatformTypes.hh" @@ -85,63 +85,12 @@ using namespace subt; class subt::GameLogicPluginPrivate { - /// \brief Mapping between artifact model names and types. - public: const std::array< - const std::pair, 14> kArtifactNames - { - { - {"backpack", subt::ArtifactType::TYPE_BACKPACK}, - {"drill", subt::ArtifactType::TYPE_DRILL}, - {"duct", subt::ArtifactType::TYPE_DUCT}, - {"electrical_box", subt::ArtifactType::TYPE_ELECTRICAL_BOX}, - {"extinguisher", subt::ArtifactType::TYPE_EXTINGUISHER}, - {"phone", subt::ArtifactType::TYPE_PHONE}, - {"radio", subt::ArtifactType::TYPE_RADIO}, - {"rescue_randy", subt::ArtifactType::TYPE_RESCUE_RANDY}, - {"toolbox", subt::ArtifactType::TYPE_TOOLBOX}, - {"valve", subt::ArtifactType::TYPE_VALVE}, - {"vent", subt::ArtifactType::TYPE_VENT}, - {"gas", subt::ArtifactType::TYPE_GAS}, - {"helmet", subt::ArtifactType::TYPE_HELMET}, - {"rope", subt::ArtifactType::TYPE_ROPE} - } - }; - - /// \brief Mapping between enum types and strings. - public: const std::array< - const std::pair, 14> kArtifactTypes - { - { - {subt::ArtifactType::TYPE_BACKPACK , "TYPE_BACKPACK"}, - {subt::ArtifactType::TYPE_DRILL , "TYPE_DRILL"}, - {subt::ArtifactType::TYPE_DUCT , "TYPE_DUCT"}, - {subt::ArtifactType::TYPE_ELECTRICAL_BOX, "TYPE_ELECTRICAL_BOX"}, - {subt::ArtifactType::TYPE_EXTINGUISHER , "TYPE_EXTINGUISHER"}, - {subt::ArtifactType::TYPE_PHONE , "TYPE_PHONE"}, - {subt::ArtifactType::TYPE_RADIO , "TYPE_RADIO"}, - {subt::ArtifactType::TYPE_RESCUE_RANDY , "TYPE_RESCUE_RANDY"}, - {subt::ArtifactType::TYPE_TOOLBOX , "TYPE_TOOLBOX"}, - {subt::ArtifactType::TYPE_VALVE , "TYPE_VALVE"}, - {subt::ArtifactType::TYPE_VENT , "TYPE_VENT"}, - {subt::ArtifactType::TYPE_GAS , "TYPE_GAS"}, - {subt::ArtifactType::TYPE_HELMET , "TYPE_HELMET"}, - {subt::ArtifactType::TYPE_ROPE , "TYPE_ROPE"} - } - }; - /// \brief Write a simulation timestamp to a logfile. /// \param[in] _simTime Current sim time. /// \return A file stream that can be used to write additional /// information to the logfile. public: std::ofstream &Log(const ignition::msgs::Time &_simTime); - /// \brief Create an ArtifactType from a string. - /// \param[in] _name The artifact in string format. - /// \param[out] _type The artifact type. - /// \return True when the conversion succeed or false otherwise. - public: bool ArtifactFromString(const std::string &_name, - subt::ArtifactType &_type); - /// \brief Calculate the score of a new artifact request. /// \param[in] _simTime Simulation time. /// \param[in] _type The object type. See ArtifactType. @@ -155,22 +104,6 @@ class subt::GameLogicPluginPrivate const ignition::msgs::Pose &_pose, subt_ros::ArtifactReport &_artifactMsg); - /// \brief Create an ArtifactType from an integer. - // - /// \param[in] _typeInt The artifact in int format. - /// \param[out] _type The artifact type. - /// \return True when the conversion succeed or false otherwise. - public: bool ArtifactFromInt(const uint32_t &_typeInt, - subt::ArtifactType &_type); - - /// \brief Create a string from ArtifactType. - // - /// \param[in] _type The artifact type. - /// \param[out] _strType The artifact string. - /// \return True when the conversion succeed or false otherwise. - public: bool StringFromArtifact(const subt::ArtifactType &_type, - std::string &_strType); - /// \brief Callback executed to process a new artifact request /// sent by a team. /// \param[in] _req The service request. @@ -1254,19 +1187,19 @@ void GameLogicPlugin::PostUpdate( // Iterate over possible artifact names for (size_t kArtifactNamesIdx = 0; - kArtifactNamesIdx < this->dataPtr->kArtifactNames.size(); + kArtifactNamesIdx < kArtifactNames.size(); ++kArtifactNamesIdx) { // If the name of the model is a possible artifact, then add it to // our list of artifacts. if (_nameComp->Data().find( - this->dataPtr->kArtifactNames[kArtifactNamesIdx].first) == 0) + kArtifactNames[kArtifactNamesIdx].first) == 0) { bool add = true; // Check to make sure the artifact has not already been added. for (const std::pair &artifactPair : this->dataPtr->artifacts[ - this->dataPtr->kArtifactNames[kArtifactNamesIdx].second]) + kArtifactNames[kArtifactNamesIdx].second]) { if (artifactPair.first == _nameComp->Data()) { @@ -1279,14 +1212,14 @@ void GameLogicPlugin::PostUpdate( { ignmsg << "Adding artifact name[" << _nameComp->Data() << "] type string[" - << this->dataPtr->kArtifactTypes[kArtifactNamesIdx].second + << kArtifactTypes[kArtifactNamesIdx].second << "] typeid[" << static_cast( - this->dataPtr->kArtifactNames[kArtifactNamesIdx].second) + kArtifactNames[kArtifactNamesIdx].second) << "]\n"; this->dataPtr->artifacts[ - this->dataPtr->kArtifactNames[ + kArtifactNames[ kArtifactNamesIdx].second][_nameComp->Data()] = ignition::math::Pose3d(ignition::math::INF_D, ignition::math::INF_D, ignition::math::INF_D, 0, 0, 0); @@ -1777,7 +1710,7 @@ bool GameLogicPluginPrivate::OnNewArtifact(const subt::msgs::Artifact &_req, } this->Finish(localSimTime); } - else if (!this->ArtifactFromInt(_req.type(), artifactType)) + else if (!ArtifactFromInt(_req.type(), artifactType)) { std::lock_guard lock(this->eventCounterMutex); std::ostringstream stream; @@ -1790,12 +1723,12 @@ bool GameLogicPluginPrivate::OnNewArtifact(const subt::msgs::Artifact &_req, this->LogEvent(stream.str()); ignerr << "Unknown artifact code. The number should be between 0 and " - << this->kArtifactTypes.size() - 1 << " but we received " + << kArtifactTypes.size() - 1 << " but we received " << _req.type() << std::endl; this->Log(localSimTime) <<"error Unknown artifact code. The number should be between " - << "0 and " << this->kArtifactTypes.size() - 1 + << "0 and " << kArtifactTypes.size() - 1 << " but we received " << _req.type() << std::endl; _resp.set_report_status("scored"); } @@ -1867,17 +1800,6 @@ bool GameLogicPluginPrivate::OnNewArtifact(const subt::msgs::Artifact &_req, return true; } -///////////////////////////////////////////////// -bool GameLogicPluginPrivate::ArtifactFromInt(const uint32_t &_typeInt, - ArtifactType &_type) -{ - if (_typeInt > this->kArtifactTypes.size()) - return false; - - _type = static_cast(_typeInt); - return true; -} - ///////////////////////////////////////////////// ignition::math::Pose3d GameLogicPluginPrivate::ConvertToArtifactOrigin( const ignition::math::Pose3d &_pose) const @@ -1916,7 +1838,7 @@ std::tuple GameLogicPluginPrivate::ScoreArtifact( // Type converted into a string. std::string reportType; - if (!this->StringFromArtifact(_type, reportType)) + if (!StringFromArtifact(_type, reportType)) { ignmsg << "Unknown artifact type" << std::endl; this->Log(_simTime) << "Unkown artifact type reported" << std::endl; @@ -2022,7 +1944,7 @@ std::tuple GameLogicPluginPrivate::ScoreArtifact( if (closestDist < 0.0 || distToArtifact < closestDist) { std::string artifactType; - if (!this->StringFromArtifact(_type, artifactType)) + if (!StringFromArtifact(_type, artifactType)) artifactType = ""; // the elements are name, type, true pos, reported pos, dist std::get<0>(this->closestReport) = artifactName; @@ -2081,44 +2003,6 @@ std::tuple GameLogicPluginPrivate::ScoreArtifact( return {score, false}; } -///////////////////////////////////////////////// -bool GameLogicPluginPrivate::ArtifactFromString(const std::string &_name, - ArtifactType &_type) -{ - auto pos = std::find_if( - std::begin(this->kArtifactTypes), - std::end(this->kArtifactTypes), - [&_name](const typename std::pair &_pair) - { - return (std::get<1>(_pair) == _name); - }); - - if (pos == std::end(this->kArtifactTypes)) - return false; - - _type = std::get<0>(*pos); - return true; -} - -///////////////////////////////////////////////// -bool GameLogicPluginPrivate::StringFromArtifact(const ArtifactType &_type, - std::string &_strType) -{ - auto pos = std::find_if( - std::begin(this->kArtifactTypes), - std::end(this->kArtifactTypes), - [&_type](const typename std::pair &_pair) - { - return (std::get<0>(_pair) == _type); - }); - - if (pos == std::end(this->kArtifactTypes)) - return false; - - _strType = std::get<1>(*pos); - return true; -} - ///////////////////////////////////////////////// void GameLogicPluginPrivate::PublishScore() { diff --git a/subt_ign/src/VisibilityRfModel.cc b/subt_ign/src/VisibilityRfModel.cc index 445ea276..b2a749cc 100644 --- a/subt_ign/src/VisibilityRfModel.cc +++ b/subt_ign/src/VisibilityRfModel.cc @@ -154,7 +154,7 @@ rf_power VisibilityModel::ComputeReceivedPower(const double &_txPower, // << ", TX: " << _txPower << ", RX: " << rx.mean << std::endl; // End option 3. - return std::move(rx); + return rx; } ///////////////////////////////////////////// diff --git a/subt_ign/src/VisibilityTable.cc b/subt_ign/src/VisibilityTable.cc index d5a1543a..418c3b55 100644 --- a/subt_ign/src/VisibilityTable.cc +++ b/subt_ign/src/VisibilityTable.cc @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -51,55 +52,12 @@ bool VisibilityTable::Load(const std::string &_worldName, bool _loadLUT) std::string worldsDirectory = SUBT_INSTALL_WORLD_DIR; this->worldName = _worldName; - // Modifications for the tunnel circuit. - const std::string tunnelPrefix = "tunnel_circuit_"; - const std::string urbanPrefix = "urban_circuit_"; - const std::string cavePrefix = "cave_circuit_"; - if (0 == this->worldName.compare(0, tunnelPrefix.size(), tunnelPrefix)) - { - std::string suffix = this->worldName.substr(tunnelPrefix.size()); - // don't use a subfolder for practice worlds - if (0 != suffix.compare(0, 9, "practice_")) - { - worldsDirectory = ignition::common::joinPaths(worldsDirectory, - "tunnel_circuit", suffix); - } - } - else if (this->worldName.find(urbanPrefix) != std::string::npos) - { - std::string suffix = this->worldName.substr(urbanPrefix.size()); - // don't use a subfolder for practice worlds - if (0 != suffix.compare(0, 9, "practice_")) - { - worldsDirectory = ignition::common::joinPaths(worldsDirectory, - "urban_circuit", suffix); - } - } - else if (this->worldName.find(cavePrefix) != std::string::npos) - { - std::string suffix = this->worldName.substr(cavePrefix.size()); - // don't use a subfolder for practice worlds - if (0 != suffix.compare(0, 9, "practice_")) - { - worldsDirectory = ignition::common::joinPaths(worldsDirectory, - "cave_circuit", suffix); - } - } - else if (this->worldName.find("simple") == std::string::npos && - this->worldName.find("_qual") == std::string::npos) - { - ignerr << "Unable to determine circuit number from[" - << this->worldName << "].\n"; - } - - this->worldPath = ignition::common::joinPaths( - worldsDirectory, _worldName + ".sdf"); - - this->graphPath = ignition::common::joinPaths( - worldsDirectory, _worldName + ".dot"); + std::string fullPath; + subt::FullWorldPath(this->worldName, fullPath); - this->lutPath = ignition::common::joinPaths( - worldsDirectory, _worldName + ".dat"); + this->worldPath = fullPath + ".sdf"; + this->graphPath = fullPath + ".dot"; + this->lutPath = fullPath + ".dat"; // Parse the .dot file and populate the world graph. if (!this->PopulateVisibilityGraph(graphPath)) diff --git a/subt_ign/src/LogChecker.cc b/subt_ign/src/apps/LogChecker.cc similarity index 100% rename from subt_ign/src/LogChecker.cc rename to subt_ign/src/apps/LogChecker.cc diff --git a/subt_ign/src/dot_generator.cc b/subt_ign/src/apps/dot_generator.cc similarity index 100% rename from subt_ign/src/dot_generator.cc rename to subt_ign/src/apps/dot_generator.cc diff --git a/subt_ign/src/level_generator.cc b/subt_ign/src/apps/level_generator.cc similarity index 100% rename from subt_ign/src/level_generator.cc rename to subt_ign/src/apps/level_generator.cc diff --git a/subt_ign/src/path_tracer.cc b/subt_ign/src/apps/path_tracer.cc similarity index 98% rename from subt_ign/src/path_tracer.cc rename to subt_ign/src/apps/path_tracer.cc index 01235509..f86bdb7d 100644 --- a/subt_ign/src/path_tracer.cc +++ b/subt_ign/src/apps/path_tracer.cc @@ -53,12 +53,6 @@ MarkerColor::MarkerColor(const YAML::Node &_node) this->emissive = loadColor(_node["emissive"]); } -////////////////////////////////////////////////// -MarkerColor::MarkerColor(const MarkerColor &_clr) - :ambient(_clr.ambient), diffuse(_clr.diffuse), emissive(_clr.emissive) -{ -} - ////////////////////////////////////////////////// MarkerColor::MarkerColor(const ignition::math::Color &_ambient, const ignition::math::Color &_diffuse, diff --git a/subt_ign/src/path_tracer.hh b/subt_ign/src/apps/path_tracer.hh similarity index 98% rename from subt_ign/src/path_tracer.hh rename to subt_ign/src/apps/path_tracer.hh index 1fbf57a9..0165ea00 100644 --- a/subt_ign/src/path_tracer.hh +++ b/subt_ign/src/apps/path_tracer.hh @@ -145,10 +145,6 @@ class MarkerColor /// \param[in] _node YAML node. public: MarkerColor(const YAML::Node &_node); - /// \brief Copy constructor - /// \param[in] _clr MarkerColor to copy. - public: MarkerColor(const MarkerColor &_clr); - /// \brief Constructor /// \param[in] _ambient Ambient color /// \param[in] _diffuse Diffuse color diff --git a/subt_ign/src/path_tracer.yml b/subt_ign/src/apps/path_tracer.yml similarity index 100% rename from subt_ign/src/path_tracer.yml rename to subt_ign/src/apps/path_tracer.yml diff --git a/subt_ign/src/validate_connections.cc b/subt_ign/src/apps/validate_connections.cc similarity index 100% rename from subt_ign/src/validate_connections.cc rename to subt_ign/src/apps/validate_connections.cc diff --git a/subt_ign/src/validate_visibility_table.cc b/subt_ign/src/apps/validate_visibility_table.cc similarity index 100% rename from subt_ign/src/validate_visibility_table.cc rename to subt_ign/src/apps/validate_visibility_table.cc diff --git a/subt_ign/src/world_generator_cave.cc b/subt_ign/src/apps/world_generator_cave.cc similarity index 99% rename from subt_ign/src/world_generator_cave.cc rename to subt_ign/src/apps/world_generator_cave.cc index 5b708529..4e4b1465 100644 --- a/subt_ign/src/world_generator_cave.cc +++ b/subt_ign/src/apps/world_generator_cave.cc @@ -1526,13 +1526,24 @@ int main(int argc, char **argv) WorldGenerator::WorldType wt; if (worldType == "a" || worldType == "anastomotic") + { wt = WorldGenerator::WorldType::CAVE_ANASTOMOTIC; + } else if (worldType == "c" || worldType == "curvilinear") + { wt = WorldGenerator::WorldType::CAVE_CURVILINEAR; + } else if (worldType == "r" || worldType == "rectilinear") + { wt = WorldGenerator::WorldType::CAVE_RECTILINEAR; - wg.SetWorldType(wt); + } + else + { + std::cerr << "One world type in [anastomoatic, curvilinear, rectilinear] must be specified\n"; + return 1; + } + wg.SetWorldType(wt); wg.Generate(); return 0; diff --git a/subt_ign/test/Common_TEST.cc b/subt_ign/test/Common_TEST.cc new file mode 100644 index 00000000..6e303958 --- /dev/null +++ b/subt_ign/test/Common_TEST.cc @@ -0,0 +1,175 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include + +///////////////////////////////////////////////// +TEST(subt_ign_Common, ArtifactTypes){ + // Tests to make sure all of our types are consistent + // + // It is expected that these tests will fail when artifacts + // are added or removed, and should be adjusted as such. + auto num_artifacts = static_cast(subt::ArtifactType::Count); + ASSERT_EQ(14u, num_artifacts); + ASSERT_EQ(num_artifacts, subt::kArtifactNames.size()); + ASSERT_EQ(num_artifacts, subt::kArtifactTypes.size()); +} + +///////////////////////////////////////////////// +TEST(subt_ign_Common, ArtifactFromInt){ + // Hardcoding to flag if definition changes + std::vector> expected = { + {0, subt::ArtifactType::TYPE_BACKPACK}, + {1, subt::ArtifactType::TYPE_DRILL}, + {2, subt::ArtifactType::TYPE_DUCT}, + {3, subt::ArtifactType::TYPE_ELECTRICAL_BOX}, + {4, subt::ArtifactType::TYPE_EXTINGUISHER}, + {5, subt::ArtifactType::TYPE_PHONE}, + {6, subt::ArtifactType::TYPE_RADIO}, + {7, subt::ArtifactType::TYPE_RESCUE_RANDY}, + {8, subt::ArtifactType::TYPE_TOOLBOX}, + {9, subt::ArtifactType::TYPE_VALVE}, + {10, subt::ArtifactType::TYPE_VENT}, + {11, subt::ArtifactType::TYPE_GAS}, + {12, subt::ArtifactType::TYPE_HELMET}, + {13, subt::ArtifactType::TYPE_ROPE} + }; + + for (const auto &[input, expected_out] : expected) + { + subt::ArtifactType type; + EXPECT_TRUE(subt::ArtifactFromInt(input, type)); + EXPECT_EQ(expected_out, type); + } + + { + // Test garbage input + subt::ArtifactType type; + EXPECT_FALSE(subt::ArtifactFromInt(-1, type)); + } + + { + // This test will fail if artifacts are added + subt::ArtifactType type; + EXPECT_FALSE(subt::ArtifactFromInt(14, type)); + } +} + +///////////////////////////////////////////////// +TEST(subt_ign_Common, ArtifactFromString){ + std::vector> expected = { + {"TYPE_BACKPACK", subt::ArtifactType::TYPE_BACKPACK}, + {"TYPE_DRILL", subt::ArtifactType::TYPE_DRILL}, + {"TYPE_DUCT", subt::ArtifactType::TYPE_DUCT}, + {"TYPE_ELECTRICAL_BOX", subt::ArtifactType::TYPE_ELECTRICAL_BOX}, + {"TYPE_EXTINGUISHER", subt::ArtifactType::TYPE_EXTINGUISHER}, + {"TYPE_PHONE", subt::ArtifactType::TYPE_PHONE}, + {"TYPE_RADIO", subt::ArtifactType::TYPE_RADIO}, + {"TYPE_RESCUE_RANDY", subt::ArtifactType::TYPE_RESCUE_RANDY}, + {"TYPE_TOOLBOX", subt::ArtifactType::TYPE_TOOLBOX}, + {"TYPE_VALVE", subt::ArtifactType::TYPE_VALVE}, + {"TYPE_VENT", subt::ArtifactType::TYPE_VENT}, + {"TYPE_GAS", subt::ArtifactType::TYPE_GAS}, + {"TYPE_HELMET", subt::ArtifactType::TYPE_HELMET}, + {"TYPE_ROPE", subt::ArtifactType::TYPE_ROPE} + }; + + for (const auto &[input, expected_out] : expected) + { + subt::ArtifactType type; + EXPECT_TRUE(subt::ArtifactFromString(input, type)); + EXPECT_EQ(expected_out, type); + } + + { + // Returns false on unknown string + subt::ArtifactType type; + EXPECT_FALSE(subt::ArtifactFromString("foobar", type)); + } +} + +///////////////////////////////////////////////// +TEST(subt_ign_Common, ArtifactFromPartialString){ + { + subt::ArtifactType type; + EXPECT_TRUE(subt::ArtifactFromPartialString("backpack_1", type)); + EXPECT_EQ(subt::ArtifactType::TYPE_BACKPACK, type); + } + { + subt::ArtifactType type; + EXPECT_TRUE(subt::ArtifactFromPartialString("rope_1", type)); + EXPECT_EQ(subt::ArtifactType::TYPE_ROPE, type); + } +} + +///////////////////////////////////////////////// +TEST(subt_ign_Common, StringFromArtifact) { + { + auto type = subt::ArtifactType::TYPE_BACKPACK; + std::string output; + EXPECT_TRUE(subt::StringFromArtifact(type, output)); + EXPECT_EQ("TYPE_BACKPACK", output); + } + { + auto type = subt::ArtifactType::TYPE_ROPE; + std::string output; + EXPECT_TRUE(subt::StringFromArtifact(type, output)); + EXPECT_EQ("TYPE_ROPE", output); + } +} + +///////////////////////////////////////////////// +TEST(subt_ign_Common, FullWorldPath) { + std::vector> expected = { + {"simple_cave_01", "/simple_cave_01"}, + {"cave_qual", "/cave_qual"}, + {"cave_circuit_practice_01", "/cave_circuit_practice_01"}, + {"cave_circuit_01", "/cave_circuit/01/cave_circuit_01"}, + + {"simple_tunnel_02", "/simple_tunnel_02"}, + {"tunnel_circuit_practice_02", "/tunnel_circuit_practice_02"}, + {"tunnel_qual_ign", "/tunnel_qual_ign"}, + {"tunnel_circuit_02", "/tunnel_circuit/02/tunnel_circuit_02"}, + + {"simple_urban_03", "/simple_urban_03"}, + {"urban_circuit_practice_03", "/urban_circuit_practice_03"}, + {"urban_qual", "/urban_qual"}, + {"urban_circuit_03", "/urban_circuit/03/urban_circuit_03"}, + }; + + for (const auto &[input, expected_out] : expected) + { + std::string worldPath; + EXPECT_TRUE(subt::FullWorldPath(input, worldPath)); + EXPECT_EQ(expected_out, worldPath); + } + + { + std::string worldPath; + EXPECT_FALSE(subt::FullWorldPath("foobar", worldPath)); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +