diff --git a/.github/workflows/asan.yaml b/.github/workflows/asan.yaml index 10bde63..63e9712 100644 --- a/.github/workflows/asan.yaml +++ b/.github/workflows/asan.yaml @@ -12,10 +12,7 @@ jobs: with: # NOTE: Avoid adding comments in the package lines, this can break some of the called scripts in github actions packages: | - rmf_building_sim_common - rmf_building_sim_gz_classic_plugins rmf_building_sim_gz_plugins rmf_robot_sim_common - rmf_robot_sim_gz_classic_plugins rmf_robot_sim_gz_plugins dist-matrix: '[{"ros_distribution": "humble", "ubuntu_distribution": "jammy"}]' diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index e51af46..0f7f748 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -12,11 +12,8 @@ jobs: with: # NOTE: Avoid adding comments in the packages lines, this can break some of the called scripts in github actions packages: | - rmf_building_sim_common - rmf_building_sim_gz_classic_plugins rmf_building_sim_gz_plugins rmf_robot_sim_common - rmf_robot_sim_gz_classic_plugins rmf_robot_sim_gz_plugins # Foxy and Galactic are deprecated for this package because of the dependency to old gazebo packages - dist-matrix: '[{"ros_distribution": "humble", "ubuntu_distribution": "jammy"}, {"ros_distribution": "rolling", "ubuntu_distribution": "jammy"}]' \ No newline at end of file + dist-matrix: '[{"ros_distribution": "jazzy", "ubuntu_distribution": "noble"}, {"ros_distribution": "rolling", "ubuntu_distribution": "noble"}]' diff --git a/.github/workflows/tsan.yaml b/.github/workflows/tsan.yaml index 3001458..1f8515b 100644 --- a/.github/workflows/tsan.yaml +++ b/.github/workflows/tsan.yaml @@ -27,11 +27,8 @@ jobs: target-ros2-distro: humble # build all packages listed in the meta package package-name: | - rmf_building_sim_common - rmf_building_sim_gz_classic_plugins rmf_building_sim_gz_plugins rmf_robot_sim_common - rmf_robot_sim_gz_classic_plugins rmf_robot_sim_gz_plugins vcs-repo-file-url: | https://raw.githubusercontent.com/open-rmf/rmf/main/rmf.repos diff --git a/rmf_building_sim_common/CHANGELOG.rst b/rmf_building_sim_common/CHANGELOG.rst deleted file mode 100644 index 57fa79b..0000000 --- a/rmf_building_sim_common/CHANGELOG.rst +++ /dev/null @@ -1,67 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rmf\_building\_sim\_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.2.2 (2023-12-20) ------------------- -* Sanitize node names to avoid plugin exceptions (`#110 `_) -* Contributors: Luca Della Vedova - -2.2.1 (2023-06-30) ------------------- - -2.2.0 (2023-06-08) ------------------- - -2.1.0 (2023-06-06) ------------------- -* Switch to rst changelogs (`#101 `_) -* Don't print warning if lift is going to same floor (`#89 `_) -* Contributors: Luca Della Vedova, Yadunund - -2.0.0 (2022-10-04) ------------------- -* Use ECM instead of traversing sdformat parent for Gazebo doors (`#80 `_) -* Change default lift current mode to AGV (`#52 `_) -* Contributors: Arjo Chakravarty, Charayaphan Nakorn Boon Han, Luca Della Vedova - -1.3.0 (2021-09-01) ------------------- -* Fix dependencies (`#26 `_) -* Changes for galactic (`#25 `_) -* Removing unnecessary qt5 dependency from CMakeLists (`#22 `_) -* Make menge mandatory and fix building\_sim dependencies (`#19 `_) -* Start using ros-tooling for build and test workflow, added coverage, tsan (`#14 `_) -* Add quality declaration documents (`#1 `_) -* Crowd step size fix for large physics step sizes (`#10 `_) -* Add build and style actions (`#11 `_) -* Slotcar plugin package move and utils cleanup (`#5 `_) -* Refactor and cleanup utils (`#6 `_) -* account for renaming from building\_map\_msgs to rmf\_building\_map\_msgs (`#3 `_) -* Contributors: Aaron Chong, Charayaphan Nakorn Boon Han, Geoffrey Biggs, Luca Della Vedova, Marco A. Gutiérrez, Yadunund - -1.2.0 (2021-01-06) ------------------- -* Add pausing feature to slotcar plugin (`#267 `_) -* dropping eloquent support (`#277 `_) -* Fix undefined initialization of \_old\_ang\_vel and \_old\_lin\_vel (`#274 `_) -* idle mode when slotcar is stationary (`#264 `_) -* Debug/slotcar battery and mode (`#261 `_) -* Added ament exports for crowd simulation common (`#260 `_) -* Make slotcar rotations follow trajectory yaw angle (`#254 `_) -* Fixed RobotMode (`#252 `_) -* Fix namespace for rmf charging plugin (`#253 `_) -* Control slotcar with model velocity cmds in place of joint velocity cmds (`#236 `_) -* Implement battery drain and recharge for slotcars (`#242 `_) -* Implement animation switching in crowd simulation (`#238 `_) -* Contributors: Geoffrey Biggs, Grey, Guoliang (Fred) Shao, Luca Della Vedova, Marco A. Gutiérrez, Morgan Quigley, Rushyendra Maganty, Yadunund, youliang - -1.1.0 (2020-09-24) ------------------- -* Crowd simulation plugin (`#218 `_) -* Improve lift initial floor definition (`#221 `_) -* Update lift session id in lift plugin (`#223 `_) -* Ignition plugins and modularization of doors and slotcar (`#138 `_) -* Adding lift plugin for ignition (`#171 `_) -* stagger door\_state publishing -* Contributors: Charayaphan Nakorn Boon Han, Guoliang (Fred) Shao, Kevin\_Skywalker, Luca Della Vedova, MakinoharaShouko diff --git a/rmf_building_sim_common/CMakeLists.txt b/rmf_building_sim_common/CMakeLists.txt deleted file mode 100644 index 19810d4..0000000 --- a/rmf_building_sim_common/CMakeLists.txt +++ /dev/null @@ -1,155 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rmf_building_sim_common) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rmf_door_msgs REQUIRED) -find_package(rmf_lift_msgs REQUIRED) -find_package(rmf_building_map_msgs REQUIRED) -find_package(menge_vendor REQUIRED) - -include(GNUInstallDirs) - -############################### -# Utils # -############################### - -add_library(rmf_building_sim_utils SHARED ${PROJECT_SOURCE_DIR}/src/utils.cpp) - -target_include_directories(rmf_building_sim_utils - PUBLIC - $ - $ -) - -############################### -# door stuff -############################### - -add_library(door_common SHARED src/door_common.cpp) - -ament_target_dependencies(door_common - rclcpp - rmf_door_msgs -) - -target_include_directories(door_common - PUBLIC - $ - $ -) - -target_link_libraries(door_common - rmf_building_sim_utils -) - -############################### -# lift stuff -############################### - -add_library(lift_common SHARED src/lift_common.cpp) - -ament_target_dependencies(lift_common - rclcpp - rmf_door_msgs - rmf_lift_msgs -) - -target_include_directories(lift_common - PUBLIC - $ - $ -) - -target_link_libraries(lift_common - rmf_building_sim_utils -) - -############################### -# crowd simulator stuff -############################### -add_library(crowd_simulator_common - SHARED - src/crowd_simulator_common.cpp -) - -target_include_directories(crowd_simulator_common - PUBLIC - $ - $ - ${menge_vendor_INCLUDE_DIRS} -) - -ament_target_dependencies(crowd_simulator_common - menge_vendor - rclcpp -) - -#crowd_simulation_common_install -ament_export_targets(crowd_simulator_common HAS_LIBRARY_TARGET) -install( - TARGETS crowd_simulator_common - EXPORT crowd_simulator_common - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -############################### -# install stuff -############################### -ament_export_dependencies( - rclcpp - rmf_door_msgs - rmf_lift_msgs - rmf_building_map_msgs - menge_vendor -) - -ament_export_include_directories(include) -ament_export_targets(rmf_building_sim_utils HAS_LIBRARY_TARGET) -ament_export_targets(door_common HAS_LIBRARY_TARGET) -ament_export_targets(lift_common HAS_LIBRARY_TARGET) - -install( - TARGETS rmf_building_sim_utils - EXPORT rmf_building_sim_utils - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -install( - TARGETS door_common - EXPORT door_common - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -install( - TARGETS lift_common - EXPORT lift_common - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -install( - DIRECTORY include/rmf_building_sim_common - DESTINATION include -) - -ament_package() diff --git a/rmf_building_sim_common/QUALITY_DECLARATION.md b/rmf_building_sim_common/QUALITY_DECLARATION.md deleted file mode 100644 index b531f21..0000000 --- a/rmf_building_sim_common/QUALITY_DECLARATION.md +++ /dev/null @@ -1,154 +0,0 @@ -This document is a declaration of software quality for the `rmf_building_sim_common` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). - -# `rmf_building_sim_common` Quality Declaration - -The package `rmf_building_sim_common` claims to be in the **Quality Level 4** category. - -Below are the detailed rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 4 in REP-2004](https://www.ros.org/reps/rep-2004.html). - -## Version Policy [1] - -### Version Scheme [1.i] - -`rmf_building_sim_common` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). - -### Version Stability [1.ii] - -`rmf_building_sim_common` is at a stable version, i.e. `>= 1.0.0`. -The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). - -### Public API Declaration [1.iii] - -All symbols in the installed headers are considered part of the public API. - -All installed headers are in the `include` directory of the package. -Headers in any other folders are not installed and are considered private. - -### API Stability Policy [1.iv] - -`rmf_building_sim_common` will not break public API within a major version number. - -### ABI Stability Policy [1.v] - -`rmf_building_sim_common` will not break public ABI within a major version number. - -### API and ABI Stability Within a Released ROS Distribution [1.vi] - -`rmf_building_sim_common` will not break public API or ABI within a released ROS distribution, i.e. no major releases into the same ROS distribution once that ROS distribution is released. - -## Change Control Process [2] - -`rmf_building_sim_common` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-requirements). - -### Change Requests [2.i] - -`rmf_building_sim_common` requires that all changes occur through a pull request. - -### Contributor Origin [2.ii] - -`rmf_building_sim_common` uses DCO as its confirmation of contributor origin policy. -More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). - -### Peer Review Policy [2.iii] - -All pull requests must have at least 1 peer review. - -### Continuous Integration [2.iv] - -All pull requests must pass CI on all platforms supported by RMF. -The CI checks only that the package builds. -The most recent CI results can be seen on [the workflow page](https://github.com/osrf/traffic_editor/actions). - -### Documentation Policy [2.v] - -All pull requests must resolve related documentation changes before merging. - -## Documentation [3] - -### Feature Documentation [3.i] - -`rmf_building_sim_common` does not provide feature documentation. - -### Public API Documentation [3.ii] - -`rmf_building_sim_common` does not provide API documentation. - -### License [3.iii] - -The license for `rmf_building_sim_common` is Apache 2.0, the type is declared in the [package.xml](package.xml) manifest file, and a full copy of the license is in the repository level [LICENSE](../LICENSE) file. - -### Copyright Statement [3.iv] - -The copyright holders each provide a statement of copyright in each source code file in `rmf_demo_tasks`. - -### Quality declaration document [3.v] - -This quality declaration is linked in the [README file](README.md). - -This quality declaration has not been externally peer-reviewed and is not registered on any Level 4 lists. - -## Testing [4] - -### Feature Testing [4.i] - -`rmf_building_sim_common` does not have any feature tests. - -### Public API Testing [4.ii] - -`rmf_building_sim_common` does not have any API tests. - -### Coverage [4.iii] - -`rmf_building_sim_common` does not track coverage statistics. - -### Performance [4.iv] - -`rmf_building_sim_common` does not have performance tests. - -### Linters and Static Analysis [4.v] - -`rmf_building_sim_common` does not use the standard linters and static analysis tools for its CMake code to ensure it follows the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters). - -## Dependencies [5] - -### Direct Runtime ROS Dependencies [5.i] - -`rmf_building_sim_common` has the following direct runtime ROS dependencies. - -#### rclcpp - -`rclcpp` is [**Quality Level 1**](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). - -#### rmf\_door\_msgs - -`rmf_door_msgs` is [**Quality Level 3**](https://github.com/open-rmf/rmf_internal_msgs/blob/main/rmf_door_msgs/QUALITY_DECLARATION.md). - -#### rmf\_lift\_msgs - -`rmf_lift_msgs` is [**Quality Level 3**](https://github.com/open-rmf/rmf_internal_msgs/blob/main/rmf_lift_msgs/QUALITY_DECLARATION.md). - -#### rmf\_building\_map\_msgs - -`rmf_building_map_msgs` is [**Quality Level 3**](https://github.com/open-rmf/rmf_building_map_msgs/blob/main/rmf_building_map_msgs/QUALITY_DECLARATION.md). - -### Optional Direct Runtime ROS Dependencies [5.ii] - -`rmf_building_sim_common` does not have any optional direct runtime ROS dependencies. - -### Direct Runtime non-ROS Dependency [5.iii] - -#### menge\_vendor - -`menge_vendor` does not declare a quality level. -It is assumed to be **Quality Level 4**. - -## Platform Support [6] - -`rmf_building_sim_common` does not support all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers). -`rmf_building_sim_common` supports ROS Eloquent and ROS Foxy. - -## Security [7] - -### Vulnerability Disclosure Policy [7.i] - -This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rmf_building_sim_common/README.md b/rmf_building_sim_common/README.md deleted file mode 100644 index 6d570ab..0000000 --- a/rmf_building_sim_common/README.md +++ /dev/null @@ -1,7 +0,0 @@ -# rmf\_building\_sim\_common - -This package provides a common library for the Ignition and Gazebo simulator plugins for simulated building infrastructure. - -## Quality Declaration - -This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](./QUALITY_DECLARATION.md) for more details. diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/crowd_simulator_common.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/crowd_simulator_common.hpp deleted file mode 100644 index 8523b0c..0000000 --- a/rmf_building_sim_common/include/rmf_building_sim_common/crowd_simulator_common.hpp +++ /dev/null @@ -1,452 +0,0 @@ -/* - * Copyright (C) 2020 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 RMF_BUILDING_SIM_COMMON__CROWD_SIMULATOR_COMMON_HPP -#define RMF_BUILDING_SIM_COMMON__CROWD_SIMULATOR_COMMON_HPP - -#include -#include -#include -#include -#include //for parsing initial_pose - -#include -#include -#include -#include - -#include - -namespace crowd_simulator { - -using AgentPtr = std::shared_ptr; - -class AgentPose3d -{ -public: - AgentPose3d() - : _x(0), _y(0), _z(0), _roll(0), _pitch(0), _yaw(0) - {} - AgentPose3d(double x, double y, double z, double roll, double pitch, - double yaw) - : _x(x), _y(y), _z(z), _roll(roll), _pitch(pitch), _yaw(yaw) - {} - - double x() const {return _x;} - double y() const {return _y;} - double z() const {return _z;} - double roll() const {return _roll;} - double pitch() const {return _pitch;} - double yaw() const {return _yaw;} - - void x(double x) {_x = x;} - void y(double y) {_y = y;} - void z(double z) {_z = z;} - void roll(double roll) {_roll = roll;} - void pitch(double pitch) {_pitch = pitch;} - void yaw(double yaw) {_yaw = yaw;} - - template - IgnMathPose3d convert_to_ign_math_pose_3d() - { - return IgnMathPose3d(_x, _y, _z, _roll, _pitch, _yaw); - } - -private: - double _x, _y, _z, _roll, _pitch, _yaw; -}; - -//================================================================ -/* -* class MengeHandle, provides a wrap-up class handle for actual menge lib -* only exposing several menge function interface -*/ -class MengeHandle : public std::enable_shared_from_this -{ -public: - - static std::shared_ptr init_and_make( - const std::string& resource_path, - const std::string& behavior_file, - const std::string& scene_file, - const float sim_time_step - ); - - MengeHandle(const std::string& resource_path, - const std::string& behavior_file, - const std::string& scene_file, - const float sim_time_step = 0.0 - ) - : _resource_path(resource_path), - _behavior_file(behavior_file), - _scene_file(scene_file), - _sim_time_step(sim_time_step), - _agent_count(0) - { - _behavior_file = this->_resource_file_path(_behavior_file); - _scene_file = this->_resource_file_path(_scene_file); - } - - void set_sim_time_step(float sim_time_step); - float get_sim_time_step() const; - size_t get_agent_count(); - void sim_step() const; //proceed one-time simulation step in _sim - AgentPtr get_agent(size_t id) const; - -private: - std::string _resource_path; - std::string _behavior_file; - std::string _scene_file; - float _sim_time_step; - size_t _agent_count; - std::shared_ptr _sim; - - std::string _resource_file_path(const std::string& relative_path) const; - bool _load_simulation(); //initialize simulatorinterface -}; - -//================================================================ -/* -* class ModelTypeDatabase -*/ -class ModelTypeDatabase -{ -public: - struct Record - { - std::string type_name; - std::string file_name; - AgentPose3d pose; - std::string animation; - std::string idle_animation; - double animation_speed; - }; - - using RecordPtr = std::shared_ptr; - - //Create a new record and returns a reference to the record - RecordPtr emplace(std::string type_name, RecordPtr record_ptr); - size_t size() const; - RecordPtr get(const std::string& type_name) const; - -private: - std::unordered_map _records; -}; - -//================================================================ -/* -* class CrowdSimInterface -* provides the relationship between menge agents and gazebo models -* provides the interface to set position between gazebo models and menge agents -*/ -class CrowdSimInterface -{ -public: - enum class AnimState - { - WALK, - IDLE, - }; - - struct Object - { - AgentPtr agent_ptr; - std::string model_name; - std::string type_name; - bool is_external = false; - AnimState current_state; - AnimState get_next_state(bool condition); - }; - using ObjectPtr = std::shared_ptr; - - CrowdSimInterface() - : _model_type_db_ptr(std::make_shared()), - _sdf_loaded(false), - _switch_anim_distance_th(0.01), - _switch_anim_name({"idle", "stand"}) - {} - - std::shared_ptr _model_type_db_ptr; - rclcpp::Logger logger() const; - void init_ros_node(const rclcpp::Node::SharedPtr node); - - bool init_crowd_sim(); - double get_sim_time_step() const; - size_t get_num_objects() const; - ObjectPtr get_object_by_id(size_t id) const; - void one_step_sim(double time_step) const; - double get_switch_anim_distance_th() const; - std::vector get_switch_anim_name() const; - bool enabled() const; - - template - bool read_sdf(SdfPtrT& sdf); - - template - void update_external_agent( - size_t id, const IgnMathPose3d& model_pose); - - template - void update_external_agent( - const AgentPtr agent_ptr, const IgnMathPose3d& model_pose); - - template - IgnMathPose3d get_agent_pose( - size_t id, double delta_sim_time); - - template - IgnMathPose3d get_agent_pose( - const AgentPtr agent_ptr, double delta_sim_time); - -private: - bool _sdf_loaded; - double _switch_anim_distance_th; - std::vector _switch_anim_name; - std::vector _objects; //Database, use id to access ObjectPtr - std::shared_ptr _menge_handle; - float _sim_time_step; - std::string _resource_path; - std::string _behavior_file; - std::string _scene_file; - std::vector _external_agents; - rclcpp::Node::SharedPtr _ros_node; - bool _enabled = false; - - template - bool _load_model_init_pose( - SdfPtrT& model_type_element, AgentPose3d& result) const; - - bool _spawn_object(); - void _add_object( - AgentPtr agent_ptr, const std::string& model_name, - const std::string& type_name, bool is_external); -}; - -template -bool CrowdSimInterface::read_sdf( - SdfPtrT& sdf) -{ - char* menge_resource_path = getenv("MENGE_RESOURCE_PATH"); - - if (menge_resource_path == nullptr || - strcmp(menge_resource_path, "") == 0) - { - RCLCPP_WARN(logger(), - "MENGE_RESOURCE_PATH env is empty. Crowd simulation is disabled."); - return true; - } - - _enabled = true; - _resource_path = std::string(menge_resource_path); - RCLCPP_INFO(logger(), - "Crowd Sim is enabled! is : %s", - _resource_path.c_str()); - - if (!sdf->HasElement("behavior_file")) - { - RCLCPP_ERROR(logger(), - "No behavior file found! Required!"); - return false; - } - _behavior_file = - sdf->GetElementImpl("behavior_file")->template Get(); - - if (!sdf->HasElement("scene_file")) - { - RCLCPP_ERROR(logger(), - "No scene file found! Required!"); - return false; - } - _scene_file = - sdf->GetElementImpl("scene_file")->template Get(); - - if (!sdf->HasElement("update_time_step")) - { - RCLCPP_ERROR(logger(), - "No update_time_step found! Required!"); - return false; - } - _sim_time_step = - sdf->GetElementImpl("update_time_step")->template Get(); - - if (!sdf->HasElement("model_type")) - { - RCLCPP_ERROR(logger(), - "No model type for agents found! element Required!"); - return false; - } - auto model_type_element = sdf->GetElementImpl("model_type"); - while (model_type_element) - { - std::string s; - if (!model_type_element->template Get("typename", s, "")) - { - RCLCPP_ERROR(logger(), - "No model type name configured in ! Required"); - return false; - } - - auto model_type_ptr = this->_model_type_db_ptr->emplace(s, - std::make_shared() ); //unordered_map - model_type_ptr->type_name = s; - - if (!model_type_element->template Get("filename", - model_type_ptr->file_name, "")) - { - RCLCPP_ERROR(logger(), - "No actor skin configured in ! Required"); - return false; - } - - if (!model_type_element->template Get("animation", - model_type_ptr->animation, "")) - { - RCLCPP_ERROR(logger(), - "No animation configured in ! Required"); - return false; - } - - if (!model_type_element->template Get("animation_speed", - model_type_ptr->animation_speed, 0.0)) - { - RCLCPP_ERROR( - logger(), - "No animation speed configured in ! Required"); - return false; - } - - if (!model_type_element->HasElement("initial_pose")) - { - RCLCPP_ERROR( - logger(), - "No model initial pose configured in ! Required [%s]", - s.c_str()); - return false; - } - if (!_load_model_init_pose(model_type_element, model_type_ptr->pose)) - { - RCLCPP_ERROR( - logger(), - "Error loading model initial pose in ! Check in [%s]", - s.c_str()); - return false; - } - - model_type_element = model_type_element->GetNextElement( - "model_type"); - } - - if (!sdf->HasElement("external_agent")) - { - RCLCPP_ERROR( - logger(), - "No external agent provided. is needed with a unique name defined above."); - } - auto external_agent_element = sdf->GetElementImpl("external_agent"); - while (external_agent_element) - { - auto ex_agent_name = external_agent_element->template Get(); - RCLCPP_INFO(logger(), - "Added external agent: [%s].", ex_agent_name.c_str()); - _external_agents.emplace_back(ex_agent_name); //just store the name - external_agent_element = external_agent_element->GetNextElement( - "external_agent"); - } - - _sdf_loaded = true; - return true; -} - -template -bool CrowdSimInterface::_load_model_init_pose( - SdfPtrT& model_type_element, AgentPose3d& result) const -{ - std::string pose_str; - if (model_type_element->template Get( - "initial_pose", pose_str, "")) - { - std::regex ws_re("\\s+"); //whitespace - std::vector parts( - std::sregex_token_iterator(pose_str.begin(), pose_str.end(), ws_re, -1), - std::sregex_token_iterator()); - - if (parts.size() != 6) - { - RCLCPP_ERROR( - logger(), - "Error loading in , 6 floats (x, y, z, pitch, roll, yaw) expected."); - return false; - } - - result.x(std::stod(parts[0]) ); - result.y(std::stod(parts[1]) ); - result.z(std::stod(parts[2]) ); - result.pitch(std::stod(parts[3]) ); - result.roll(std::stod(parts[4]) ); - result.yaw(std::stod(parts[5]) ); - } - return true; -} - -template -IgnMathPose3d CrowdSimInterface::get_agent_pose( - size_t id, double delta_sim_time) -{ - assert(id < get_num_objects()); - auto agent_ptr = _objects[id]->agent_ptr; - return get_agent_pose(agent_ptr, delta_sim_time); -} - -template -IgnMathPose3d CrowdSimInterface::get_agent_pose( - const AgentPtr agent_ptr, double delta_sim_time) -{ - //calculate future position in delta_sim_time. currently in 2d - assert(agent_ptr); - double px = static_cast(agent_ptr->_pos.x()) + - static_cast(agent_ptr->_vel.x()) * delta_sim_time; - double py = static_cast(agent_ptr->_pos.y()) + - static_cast(agent_ptr->_vel.y()) * delta_sim_time; - - double x_rot = static_cast(agent_ptr->_orient.x()); - double y_rot = static_cast(agent_ptr->_orient.y()); - - IgnMathPose3d agent_pose(px, py, 0, 0, 0, std::atan2(y_rot, x_rot)); - return agent_pose; -} - -template -void CrowdSimInterface::update_external_agent( - size_t id, const IgnMathPose3d& model_pose) -{ - assert(id < get_num_objects()); - auto agent_ptr = _objects[id]->agent_ptr; - update_external_agent(agent_ptr, model_pose); -} - -template -void CrowdSimInterface::update_external_agent( - const AgentPtr agent_ptr, const IgnMathPose3d& model_pose) -{ - assert(agent_ptr); - agent_ptr->_pos.setX(model_pose.Pos().X()); - agent_ptr->_pos.setY(model_pose.Pos().Y()); -} - -} // namespace crowd_simulator - -#endif // CROWD_SIMULATION_COMMON__CROWD_SIMULATOR_COMMON_HPP diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp deleted file mode 100644 index 0476cfa..0000000 --- a/rmf_building_sim_common/include/rmf_building_sim_common/door_common.hpp +++ /dev/null @@ -1,301 +0,0 @@ -/* - * Copyright (C) 2020 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 RMF_BUILDING_SIM_COMMON__DOOR_COMMON_HPP -#define RMF_BUILDING_SIM_COMMON__DOOR_COMMON_HPP - -#include -#include - -#include -#include -#include - -#include "utils.hpp" - -#include -#include -#include - -namespace rmf_building_sim_common { - -using DoorMode = rmf_door_msgs::msg::DoorMode; -using DoorState = rmf_door_msgs::msg::DoorState; -using DoorRequest = rmf_door_msgs::msg::DoorRequest; - -//============================================================================== -class DoorCommon -{ - -public: - - struct DoorUpdateRequest - { - std::string joint_name; - double position; - double velocity; - }; - - struct DoorUpdateResult - { - std::string joint_name; - double velocity; - double fmax; - }; - - template - static std::shared_ptr make( - const std::string& door_name, - rclcpp::Node::SharedPtr node, - SdfPtrT& sdf); - - rclcpp::Logger logger() const; - - std::vector joint_names() const; - - MotionParams& params(); - - std::vector update(const double time, - const std::vector& request); - - - struct DoorElement - { - double closed_position; - double open_position; - double current_position; - double current_velocity; - - DoorElement() {} - - DoorElement( - const double lower_limit, - const double upper_limit, - const bool flip_direction = false) - : current_position(0.0), - current_velocity(0.0) - { - if (flip_direction) - { - closed_position = lower_limit; - open_position = upper_limit; - } - else - { - closed_position = upper_limit; - open_position = lower_limit; - } - } - }; - - // Map joint name to its DoorElement - using Doors = std::unordered_map; - - template - static std::shared_ptr make( - const std::string& door_name, - rclcpp::Node::SharedPtr node, - SdfPtrT& sdf, - Doors& doors); - -private: - DoorMode requested_mode() const; - - void publish_state(const uint32_t door_value, const rclcpp::Time& time); - - double calculate_target_velocity( - const double target, - const double current_position, - const double current_velocity, - const double dt); - - DoorCommon(const std::string& door_name, - rclcpp::Node::SharedPtr node, - const MotionParams& params, - const Doors& doors); - - bool all_doors_open(); - - bool all_doors_closed(); - - rclcpp::Node::SharedPtr _ros_node; - rclcpp::Publisher::SharedPtr _door_state_pub; - rclcpp::Subscription::SharedPtr _door_request_sub; - - DoorState _state; - DoorRequest _request; - - MotionParams _params; - - double _last_update_time = 0.0; - // random start time offset to prevent state message crossfire - double _last_pub_time = ((double) std::rand()) / ((double) (RAND_MAX)); - - bool _initialized = false; - - // Map of joint_name and corresponding DoorElement - Doors _doors; -}; - -template -std::shared_ptr DoorCommon::make( - const std::string& door_name, - rclcpp::Node::SharedPtr node, - SdfPtrT& sdf) -{ - // We work with a clone to avoid const correctness issues with - // get_sdf_param functions in utils.hpp - auto sdf_clone = sdf->Clone(); - - MotionParams params; - get_sdf_param_if_available(sdf_clone, "v_max_door", params.v_max); - get_sdf_param_if_available(sdf_clone, "a_max_door", params.a_max); - get_sdf_param_if_available(sdf_clone, "a_nom_door", params.a_nom); - get_sdf_param_if_available(sdf_clone, "dx_min_door", params.dx_min); - get_sdf_param_if_available(sdf_clone, "f_max_door", params.f_max); - - auto door_element = sdf_clone; - std::string left_door_joint_name; - std::string right_door_joint_name; - std::string door_type; - - // Get the joint names and door type - if (!get_element_required(sdf_clone, "door", door_element) || - !get_sdf_attribute_required( - door_element, "left_joint_name", left_door_joint_name) || - !get_sdf_attribute_required( - door_element, "right_joint_name", right_door_joint_name) || - !get_sdf_attribute_required( - door_element, "type", door_type)) - { - RCLCPP_ERROR(node->get_logger(), - " -- Missing required parameters for [%s] plugin", - door_name.c_str()); - return nullptr; - } - - if ((left_door_joint_name == "empty_joint" && - right_door_joint_name == "empty_joint") || - (left_door_joint_name.empty() && right_door_joint_name.empty())) - { - RCLCPP_ERROR(node->get_logger(), - " -- Both door joint names are missing for [%s] plugin, at least one" - " is required", door_name.c_str()); - return nullptr; - } - - std::unordered_set joint_names; - if (!left_door_joint_name.empty() && left_door_joint_name != "empty_joint") - joint_names.insert(left_door_joint_name); - if (!right_door_joint_name.empty() && right_door_joint_name != "empty_joint") - joint_names.insert(right_door_joint_name); - - Doors doors; - - auto extract_door = [&](SdfPtrT& joint_sdf) - { - auto joint_sdf_clone = joint_sdf->Clone(); - std::string joint_name; - get_sdf_attribute_required( - joint_sdf_clone, "name", joint_name); - const auto it = joint_names.find(joint_name); - if (it != joint_names.end()) - { - auto element = joint_sdf_clone; - get_element_required(joint_sdf_clone, "axis", element); - get_element_required(element, "limit", element); - double lower_limit = -1.57; - double upper_limit = 0.0; - get_sdf_param_if_available(element, "lower", lower_limit); - get_sdf_param_if_available(element, "upper", upper_limit); - DoorCommon::DoorElement door_element; - if (joint_name == right_door_joint_name) - door_element = - DoorCommon::DoorElement{lower_limit, upper_limit, true} - ; - else if (joint_name == left_door_joint_name) - door_element = DoorCommon::DoorElement{lower_limit, upper_limit} - ; - doors.insert({joint_name, door_element}); - } - }; - - // Get the joint limits from parent sdf - auto parent = sdf->GetParent(); - if (!parent) - { - RCLCPP_ERROR(node->get_logger(), - "Unable to access parent sdf to retrieve joint limits"); - return nullptr; - } - - auto joint_element = parent->GetElement("joint"); - if (!joint_element) - { - RCLCPP_ERROR(node->get_logger(), - "Parent sdf missing required joint element"); - return nullptr; - } - - extract_door(joint_element); - // Find next joint element if present - while (joint_element) - { - extract_door(joint_element); - joint_element = joint_element->GetNextElement("joint"); - } - - std::shared_ptr door_common(new DoorCommon( - door_name, - node, - params, - doors)); - - return door_common; - -} - -template -std::shared_ptr DoorCommon::make( - const std::string& door_name, - rclcpp::Node::SharedPtr node, - SdfPtrT& sdf, - Doors& doors) -{ - // We work with a clone to avoid const correctness issues with - // get_sdf_param functions in utils.hpp - auto sdf_clone = sdf->Clone(); - - MotionParams params; - get_sdf_param_if_available(sdf_clone, "v_max_door", params.v_max); - get_sdf_param_if_available(sdf_clone, "a_max_door", params.a_max); - get_sdf_param_if_available(sdf_clone, "a_nom_door", params.a_nom); - get_sdf_param_if_available(sdf_clone, "dx_min_door", params.dx_min); - get_sdf_param_if_available(sdf_clone, "f_max_door", params.f_max); - - std::shared_ptr door_common(new DoorCommon( - door_name, - node, - params, - doors)); - - return door_common; -} - -} // namespace rmf_building_sim_common - -#endif // RMF_BUILDING_SIM_COMMON__DOOR_COMMON_HPP diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp deleted file mode 100644 index ca601e4..0000000 --- a/rmf_building_sim_common/include/rmf_building_sim_common/lift_common.hpp +++ /dev/null @@ -1,266 +0,0 @@ -/* - * Copyright (C) 2020 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 RMF_BUILDING_SIM_COMMON__LIFT_COMMON_HPP -#define RMF_BUILDING_SIM_COMMON__LIFT_COMMON_HPP - -#include -#include - -#include -#include -#include -#include -#include - -#include "utils.hpp" - -#include -#include -#include - -namespace rmf_building_sim_common { - -using LiftState = rmf_lift_msgs::msg::LiftState; -using LiftRequest = rmf_lift_msgs::msg::LiftRequest; -using DoorRequest = rmf_door_msgs::msg::DoorRequest; -using DoorState = rmf_door_msgs::msg::DoorState; -using DoorMode = rmf_door_msgs::msg::DoorMode; - -//============================================================================== -class LiftCommon -{ - -public: - - struct LiftUpdateResult - { - double velocity; - double fmax; - }; - - template - static std::unique_ptr make( - const std::string& lift_name, - rclcpp::Node::SharedPtr node, - SdfPtrT& sdf); - - rclcpp::Logger logger() const; - - LiftUpdateResult update(const double time, const double position, - const double velocity); - - std::string get_joint_name() const; - - double get_elevation() const; - - bool motion_state_changed(); - -private: - - rclcpp::Node::SharedPtr _ros_node; - rclcpp::Publisher::SharedPtr _lift_state_pub; - rclcpp::Publisher::SharedPtr _door_request_pub; - rclcpp::Subscription::SharedPtr _lift_request_sub; - rclcpp::Subscription::SharedPtr _door_state_sub; - - std::string _lift_name; - std::string _cabin_joint_name; - - MotionParams _cabin_motion_params; - LiftState::_motion_state_type _old_motion_state; - - std::vector _floor_names; - std::unordered_map _floor_name_to_elevation; - std::unordered_map> _floor_name_to_shaft_door_name; - std::unordered_map> _floor_name_to_cabin_door_name; - std::unordered_map _shaft_door_states; - std::unordered_map _cabin_door_states; - - LiftState _lift_state; - LiftRequest::UniquePtr _lift_request; - - double _last_update_time = 0.0; - // random start time offset to prevent state message crossfire - double _last_pub_time = ((double) std::rand()) / ((double) (RAND_MAX)); - - void publish_door_request(const double time, std::string door_name, - uint32_t door_state); - - LiftCommon(rclcpp::Node::SharedPtr node, - const std::string& lift_name, - const std::string& joint_name, - const MotionParams& cabin_motion_params, - const std::vector& floor_names, - const std::unordered_map& floor_name_to_elevation, - std::unordered_map< - std::string, std::vector> floor_name_to_shaft_door_name, - std::unordered_map< - std::string, std::vector> floor_name_to_cabin_door_name, - std::unordered_map shaft_door_states, - std::unordered_map cabin_door_states, - std::string initial_floor_name); - - double get_step_velocity(const double dt, const double position, - const double velocity); - - void update_cabin_state(const double position, const double velocity); - - void move_doors(const double time, uint32_t door_mode); - - void open_doors(const double time); - - void close_doors(const double time); - - uint32_t get_door_state( - const std::unordered_map>& floor_to_door_map, - const std::unordered_map& door_states); - - void pub_lift_state(const double time); - - void update_lift_door_state(); - -}; - -template -std::unique_ptr LiftCommon::make( - const std::string& lift_name, - rclcpp::Node::SharedPtr node, - SdfPtrT& sdf) -{ - MotionParams cabin_motion_params; - std::string joint_name; - std::vector floor_names; - std::unordered_map floor_name_to_elevation; - std::unordered_map> floor_name_to_shaft_door_name; - std::unordered_map> floor_name_to_cabin_door_name; - std::unordered_map shaft_door_states; - std::unordered_map cabin_door_states; - - - auto sdf_clone = sdf->Clone(); - - // load lift cabin motion parameters - get_sdf_param_if_available(sdf_clone, "v_max_cabin", - cabin_motion_params.v_max); - get_sdf_param_if_available(sdf_clone, "a_max_cabin", - cabin_motion_params.a_max); - get_sdf_param_if_available(sdf_clone, "a_nom_cabin", - cabin_motion_params.a_nom); - get_sdf_param_if_available(sdf_clone, "dx_min_cabin", - cabin_motion_params.dx_min); - get_sdf_param_if_available(sdf_clone, "f_max_cabin", - cabin_motion_params.f_max); - if (!get_sdf_param_required(sdf_clone, "cabin_joint_name", - joint_name)) - return nullptr; - - // load the floor name and elevation for each floor - auto floor_element = sdf_clone; - if (!get_element_required(sdf, "floor", floor_element)) - { - RCLCPP_ERROR(node->get_logger(), - " -- Missing required floor element for [%s] plugin", - lift_name.c_str()); - return nullptr; - } - - while (floor_element) - { - std::string floor_name; - double floor_elevation; - if (!get_sdf_attribute_required(floor_element, "name", - floor_name) || - !get_sdf_attribute_required(floor_element, "elevation", - floor_elevation)) - { - RCLCPP_ERROR( - node->get_logger(), - " -- Missing required floor name or elevation attributes for [%s] plugin", - lift_name.c_str()); - return nullptr; - } - floor_names.push_back(floor_name); - floor_name_to_elevation.insert({floor_name, floor_elevation}); - - auto door_pair_element = floor_element; - if (get_element_required(floor_element, "door_pair", door_pair_element)) - { - while (door_pair_element) - { - std::string shaft_door_name; - std::string cabin_door_name; - if (!get_sdf_attribute_required(door_pair_element, - "cabin_door", cabin_door_name) || - !get_sdf_attribute_required(door_pair_element, - "shaft_door", shaft_door_name)) - { - RCLCPP_ERROR(node->get_logger(), - " -- Missing required lift door attributes for [%s] plugin", - lift_name.c_str()); - return nullptr; - } - floor_name_to_cabin_door_name[floor_name].push_back(cabin_door_name); - floor_name_to_shaft_door_name[floor_name].push_back(shaft_door_name); - shaft_door_states.insert({shaft_door_name, nullptr}); - cabin_door_states.insert({cabin_door_name, nullptr}); - - door_pair_element = door_pair_element->GetNextElement("door_pair"); - } - } - floor_element = floor_element->GetNextElement("floor"); - } - - assert(!floor_names.empty()); - std::string initial_floor_name = floor_names[0]; - get_sdf_param_if_available(sdf_clone, "initial_floor", - initial_floor_name); - - if (std::find(floor_names.begin(), floor_names.end(), initial_floor_name) == - floor_names.end()) - { - RCLCPP_WARN( - node->get_logger(), - "Initial floor [%s] is not available, changing to deafult", - initial_floor_name.c_str()); - initial_floor_name = floor_names[0]; - } - - std::unique_ptr lift(new LiftCommon( - node, - lift_name, - joint_name, - cabin_motion_params, - floor_names, - floor_name_to_elevation, - floor_name_to_shaft_door_name, - floor_name_to_cabin_door_name, - shaft_door_states, - cabin_door_states, - initial_floor_name)); - - return lift; -} - -} // namespace rmf_building_sim_common - -#endif // RMF_BUILDING_SIM_COMMON__LIFT_COMMON_HPP diff --git a/rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp b/rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp deleted file mode 100644 index c69ec9e..0000000 --- a/rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp +++ /dev/null @@ -1,138 +0,0 @@ -#ifndef SRC__RMF_BUILDING_SIM_COMMON__UTILS_HPP -#define SRC__RMF_BUILDING_SIM_COMMON__UTILS_HPP - -#include -#include - -namespace rmf_building_sim_common { - -struct MotionParams -{ - double v_max = 0.2; - double a_max = 0.1; - double a_nom = 0.08; - double dx_min = 0.01; - double f_max = 10000000.0; -}; - -//============================================================================== -// TODO(MXG): Refactor the use of this function to replace it with -// compute_desired_rate_of_change() -double compute_ds( - double s_target, - double v_actual, - const double v_max, - const double accel_nom, - const double accel_max, - const double dt); - -//============================================================================== -double compute_desired_rate_of_change( - double _s_target, - double _v_actual, - const MotionParams& _motion_params, - const double _dt); - -//============================================================================== -void sanitize_node_name(std::string& node_name); - -//============================================================================== -template -bool get_element_required( - SdfPtrT& _sdf, - const std::string& _element_name, - SdfElementPtrT& _element) -{ - if (!_sdf->HasElement(_element_name)) - { - std::cerr << "Element [" << _element_name << "] not found" << std::endl; - return false; - } - // using GetElementImpl() because for sdf::v9 GetElement() is not const - _element = _sdf->GetElementImpl(_element_name); - return true; -} - -//============================================================================== -template -bool get_sdf_attribute_required(SdfPtrT& sdf, const std::string& attribute_name, - T& value) -{ - if (sdf->HasAttribute(attribute_name)) - { - if (sdf->GetAttribute(attribute_name)->Get(value)) - { - std::cout << "Using specified attribute value [" << value - << "] for property [" << attribute_name << "]" - << std::endl; - return true; - } - else - { - std::cerr << "Failed to parse sdf attribute for [" << attribute_name - << "]" << std::endl; - } - } - else - { - std::cerr << "Attribute [" << attribute_name << "] not found" << std::endl; - } - - return false; -} - -//============================================================================== -template -bool get_sdf_param_required(SdfPtrT& sdf, const std::string& parameter_name, - T& value) -{ - if (sdf->HasElement(parameter_name)) - { - if (sdf->GetElement(parameter_name)->GetValue()->Get(value)) - { - std::cout << "Using specified value [" << value << "] for property [" - << parameter_name << "]" << std::endl; - return true; - } - else - { - std::cerr << "Failed to parse sdf value for [" << parameter_name << "]" - < -void get_sdf_param_if_available(SdfPtrT& sdf, const std::string& parameter_name, - T& value) -{ - if (sdf->HasElement(parameter_name)) - { - if (sdf->GetElement(parameter_name)->GetValue()->Get(value)) - { - std::cout << "Using specified value [" << value << "] for property [" - << parameter_name << "]" << std::endl; - } - else - { - std::cerr << "Failed to parse sdf value for [" << parameter_name - << "]" << std::endl; - } - } - else - { - std::cout << "Using default value [" << value << "] for property [" - << parameter_name << "]" << std::endl; - } -} - -} // namespace rmf_building_sim_common - -#endif // SRC__RMF_BUILDING_SIM_COMMON__UTILS_HPP diff --git a/rmf_building_sim_common/package.xml b/rmf_building_sim_common/package.xml deleted file mode 100644 index 3dd657d..0000000 --- a/rmf_building_sim_common/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rmf_building_sim_common - 2.2.2 - - Common utility functions for Gazebo-classic and Gazebo building plugins - - Morgan Quigley - Michael Grey - Roselle Carmen - Aaron Chong - Brandon Ong - Kevin Ma - Rushyendra Maganty - Yadunund - Morgan Quigley - Marco A. Gutiérrez - Apache 2.0 - - ament_cmake - - rclcpp - rmf_door_msgs - rmf_lift_msgs - rmf_building_map_msgs - menge_vendor - - - ament_cmake - - - diff --git a/rmf_building_sim_common/src/crowd_simulator_common.cpp b/rmf_building_sim_common/src/crowd_simulator_common.cpp deleted file mode 100644 index 89bb9fb..0000000 --- a/rmf_building_sim_common/src/crowd_simulator_common.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/* - * Copyright (C) 2020 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 - -#include - -namespace crowd_simulator { - -//================================================================ -std::shared_ptr MengeHandle::init_and_make( - const std::string& resource_path, - const std::string& behavior_file, - const std::string& scene_file, - const float sim_time_step -) -{ - auto menge_handle = std::make_shared( - resource_path, behavior_file, scene_file, sim_time_step); - if (!menge_handle->_load_simulation()) - { - return nullptr; - } - return menge_handle; -} - -void MengeHandle::set_sim_time_step(float sim_time_step) -{ - this->_sim_time_step = sim_time_step; - // Set it if a valid handle is present - if (this->_sim) - { - this->_sim->setTimeStep(sim_time_step); - } -} - -float MengeHandle::get_sim_time_step() const -{ - return this->_sim_time_step; -} - -size_t MengeHandle::get_agent_count() -{ - if (this->_agent_count == 0) - { - this->_agent_count = this->_sim->getNumAgents(); - } - return this->_agent_count; -} - -void MengeHandle::sim_step() const -{ - this->_sim->step(); -} - -AgentPtr MengeHandle::get_agent(size_t id) const -{ - return AgentPtr(this->_sim->getAgent(id)); -} - -std::string MengeHandle::_resource_file_path(const std::string& relative_path) -const -{ - std::string full_path = this->_resource_path + "/" + relative_path; - std::cout << "Finding resource file: " << full_path << std::endl; - std::ifstream ifile(full_path); - if (!static_cast(ifile)) - { - std::cerr << "File not found! " << full_path << std::endl; - assert(static_cast(ifile)); - } - std::cout << "Found." << std::endl; - return full_path; -} - -bool MengeHandle::_load_simulation() -{ - Menge::SimulatorDB sim_db; - Menge::PluginEngine::CorePluginEngine engine(&sim_db); - - std::cout << "Start CrowdSimulator initializing [Menge]..." << std::endl; - - this->_sim = std::shared_ptr( - sim_db.getDBEntry("orca")->getSimulator( - this->_agent_count, - this->_sim_time_step, - 0, - std::numeric_limits::infinity(), - this->_behavior_file, - this->_scene_file, - "", - "", - false) - ); - - if (this->_sim) - { - std::cout << std::endl << "Crowd Simulator initialized success [Menge]. " << - std::endl; - return true; - } - std::cout << - "Error in provided navmesh. Menge simulator initialized false." << - std::endl; - return false; -} - -//============================================ -ModelTypeDatabase::RecordPtr ModelTypeDatabase::emplace( - std::string type_name, - RecordPtr record_ptr) -{ - auto pair = this->_records.emplace(type_name, record_ptr); //return pair - assert(pair.second); - return pair.first->second; -} - -ModelTypeDatabase::RecordPtr ModelTypeDatabase::get( - const std::string& type_name) -const -{ - auto it = this->_records.find(type_name); - if (it == this->_records.end()) - { - std::cout << "The model type [ " << type_name << - " ] is not defined in scene file!" << std::endl; - return nullptr; - } - return it->second; -} - -size_t ModelTypeDatabase::size() const -{ - return this->_records.size(); -} - -//================================================================ - -rclcpp::Logger CrowdSimInterface::logger() const -{ - return rclcpp::get_logger("crowdsim"); -} - -void CrowdSimInterface::init_ros_node(const rclcpp::Node::SharedPtr node) -{ - _ros_node = std::move(node); -} - -bool CrowdSimInterface::enabled() const -{ - return _enabled; -} - -bool CrowdSimInterface::init_crowd_sim() -{ - _menge_handle = MengeHandle::init_and_make( - _resource_path, - _behavior_file, - _scene_file, - _sim_time_step); - - if (!_sdf_loaded) - { - RCLCPP_ERROR( - logger(), - "Please load the sdf before initialize the crowd_sim interface!"); - return false; - } - _spawn_object(); - return true; -} - -double CrowdSimInterface::get_sim_time_step() const -{ - return static_cast(_sim_time_step); -} - -bool CrowdSimInterface::_spawn_object() -{ - //External models are loaded first in scene file - size_t external_count = _external_agents.size(); - size_t total_agent_count = _menge_handle->get_agent_count(); - - //external model must be included in scene file - assert(external_count <= total_agent_count); - - for (size_t i = 0; i < external_count; ++i) - { - auto agent_ptr = _menge_handle->get_agent(i); - agent_ptr->_external = true; - _add_object(agent_ptr, _external_agents[i], "0", true); - } - - for (size_t i = external_count; i < total_agent_count; ++i) - { - auto agent_ptr = this->_menge_handle->get_agent(i); - agent_ptr->_external = false; - std::string model_name = "agent" + std::to_string(i); - _add_object(agent_ptr, model_name, agent_ptr->_typeName, false); - } - return true; -} - -void CrowdSimInterface::_add_object(AgentPtr agent_ptr, - const std::string& model_name, - const std::string& type_name, - bool is_external = false) -{ - assert(agent_ptr); - // must provide a model name in gazebo if it's an external agent - if (is_external) - { - assert(!model_name.empty()); - } - _objects.emplace_back( - new Object{agent_ptr, model_name, type_name, is_external, - AnimState::WALK}); -} - -size_t CrowdSimInterface::get_num_objects() const -{ - return _objects.size(); -} - -CrowdSimInterface::ObjectPtr CrowdSimInterface::get_object_by_id(size_t id) -const -{ - assert(id < _objects.size()); - return _objects[id]; -} - -void CrowdSimInterface::one_step_sim(double time_step) const -{ - _menge_handle->set_sim_time_step(time_step); - _menge_handle->sim_step(); -} - -double CrowdSimInterface::get_switch_anim_distance_th() const -{ - return _switch_anim_distance_th; -} - -std::vector CrowdSimInterface::get_switch_anim_name() const -{ - return _switch_anim_name; -} - -//============================================= -CrowdSimInterface::AnimState CrowdSimInterface::Object::get_next_state( - bool condition) -{ - if (condition) - return AnimState::IDLE; - else - return AnimState::WALK; - - return current_state; -} - -} //namespace crowd_simulator diff --git a/rmf_building_sim_common/src/door_common.cpp b/rmf_building_sim_common/src/door_common.cpp deleted file mode 100644 index 67785db..0000000 --- a/rmf_building_sim_common/src/door_common.cpp +++ /dev/null @@ -1,201 +0,0 @@ -/* - * Copyright (C) 2020 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 - -using namespace std::chrono_literals; - -namespace rmf_building_sim_common { - -rclcpp::Logger DoorCommon::logger() const -{ - return rclcpp::get_logger("door_" + _state.door_name); -} - -DoorMode DoorCommon::requested_mode() const -{ - return _request.requested_mode; -} - -std::vector DoorCommon::joint_names() const -{ - std::vector joint_names; - for (const auto& door : _doors) - joint_names.push_back(door.first); - - return joint_names; -} - -MotionParams& DoorCommon::params() -{ - return _params; -} - -void DoorCommon::publish_state(const uint32_t door_value, - const rclcpp::Time& time) -{ - if (!_initialized) - return; - - _state.current_mode.value = door_value; - _state.door_time = time; - _door_state_pub->publish(_state); -} - - -DoorCommon::DoorCommon(const std::string& door_name, - rclcpp::Node::SharedPtr node, - const MotionParams& params, - const Doors& doors) -: _ros_node(std::move(node)), - _params(params), - _doors(doors) -{ - _state.door_name = door_name; - _request.requested_mode.value = DoorMode::MODE_CLOSED; - - _door_state_pub = _ros_node->create_publisher( - "/door_states", rclcpp::SystemDefaultsQoS().keep_last(10)); - - _door_request_sub = _ros_node->create_subscription( - "/door_requests", rclcpp::SystemDefaultsQoS().keep_last(10), - [&](DoorRequest::UniquePtr msg) - { - if (msg->door_name == _state.door_name) - _request = *msg; - }); - - _initialized = true; -} - -bool DoorCommon::all_doors_open() -{ - for (const auto& door : _doors) - if (std::abs(door.second.open_position - - door.second.current_position) > _params.dx_min) - return false; - - - - return true; -} - -bool DoorCommon::all_doors_closed() -{ - for (const auto& door : _doors) - if (std::abs(door.second.closed_position - - door.second.current_position) > _params.dx_min) - return false; - - - - return true; -} - -double DoorCommon::calculate_target_velocity( - const double target, - const double current_position, - const double current_velocity, - const double dt) -{ - double dx = target - current_position; - if (std::abs(dx) < _params.dx_min/2.0) - dx = 0.0; - - double door_v = compute_desired_rate_of_change( - dx, current_velocity, _params, dt); - - return door_v; -} - -std::vector DoorCommon::update( - const double time, - const std::vector& requests) -{ - double dt = time - _last_update_time; - _last_update_time = time; - - // Update simulation position and velocity of each joint and - // calcuate target velocity for the same - std::vector results; - for (const auto& request : requests) - { - const auto it = _doors.find(request.joint_name); - if (it != _doors.end()) - { - it->second.current_position = request.position; - it->second.current_velocity = request.velocity; - DoorCommon::DoorUpdateResult result; - result.joint_name = request.joint_name; - result.fmax = _params.f_max; - if (requested_mode().value == DoorMode::MODE_OPEN) - { - result.velocity = calculate_target_velocity( - it->second.open_position, - request.position, - request.velocity, - dt); - } - else - { - result.velocity = calculate_target_velocity( - it->second.closed_position, - request.position, - request.velocity, - dt); - } - results.push_back(result); - } - else - { - RCLCPP_ERROR(logger(), - "Received update request for uninitialized joint [%s]", - request.joint_name.c_str()); - } - } - - // Publishing door states - if (time - _last_pub_time >= 1.0) - { - _last_pub_time = time; - const int32_t t_sec = static_cast(time); - const uint32_t t_nsec = - static_cast((time-static_cast(t_sec)) *1e9); - const rclcpp::Time now{t_sec, t_nsec, RCL_ROS_TIME}; - - if (all_doors_open()) - { - publish_state(DoorMode::MODE_OPEN, now); - } - else if (all_doors_closed()) - { - publish_state(DoorMode::MODE_CLOSED, now); - } - else - { - publish_state(DoorMode::MODE_MOVING, now); - } - } - - return results; -} - - -} // namespace rmf_building_sim_common diff --git a/rmf_building_sim_common/src/lift_common.cpp b/rmf_building_sim_common/src/lift_common.cpp deleted file mode 100644 index a0d56c2..0000000 --- a/rmf_building_sim_common/src/lift_common.cpp +++ /dev/null @@ -1,358 +0,0 @@ -/* - * Copyright (C) 2020 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 - -namespace rmf_building_sim_common { - -rclcpp::Logger LiftCommon::logger() const -{ - return rclcpp::get_logger("lift_" + _lift_state.lift_name); -} - -std::string LiftCommon::get_joint_name() const -{ - return _cabin_joint_name; -} - -double LiftCommon::get_elevation() const -{ - return _floor_name_to_elevation.at(_lift_state.destination_floor); -} - -void LiftCommon::publish_door_request(const double time, std::string door_name, - uint32_t door_state) -{ - DoorRequest request; - request.request_time = rclcpp::Time(time); - request.requester_id = _lift_name; - request.door_name = door_name; - request.requested_mode.value = door_state; - - _door_request_pub->publish(request); -} - -double LiftCommon::get_step_velocity(const double dt, const double position, - const double velocity) -{ - double desired_elevation = get_elevation(); - double dz = desired_elevation - position; - - if (abs(dz) < _cabin_motion_params.dx_min / 2.0) - dz = 0; - - return compute_desired_rate_of_change( - dz, velocity, _cabin_motion_params, dt); -} - -// Compares current lift motion state to the last time this function was called -bool LiftCommon::motion_state_changed() -{ - bool changed = _lift_state.motion_state != _old_motion_state; - _old_motion_state = _lift_state.motion_state; - return changed; -} - -void LiftCommon::update_cabin_state(const double position, - const double velocity) -{ - // TODO update current_floor only when lift reaches its destination - double smallest_error = std::numeric_limits::max(); - std::string closest_floor_name; - for (const auto& floor : _floor_name_to_elevation) - { - double tmp_error = abs(position - floor.second); - if (tmp_error < smallest_error) - { - smallest_error = tmp_error; - closest_floor_name = floor.first; - } - } - _lift_state.current_floor = closest_floor_name; - - // Set motion state - if (abs(velocity) < 0.01) - _lift_state.motion_state = LiftState::MOTION_STOPPED; - else if (velocity > 0) - _lift_state.motion_state = LiftState::MOTION_UP; - else - _lift_state.motion_state = LiftState::MOTION_DOWN; -} - -void LiftCommon::move_doors(const double time, uint32_t door_mode) -{ - auto cabin_door_names = - _floor_name_to_cabin_door_name[_lift_state.current_floor]; - for (const auto& cabin_door : cabin_door_names) - { - const auto it = _cabin_door_states.find(cabin_door); - if (it == _cabin_door_states.end()) - continue; - if (it->second && it->second->current_mode.value != door_mode) - publish_door_request(time, cabin_door, door_mode); - } - auto shaft_door_names = - _floor_name_to_shaft_door_name[_lift_state.current_floor]; - for (const auto& shaft_door : shaft_door_names) - { - const auto it = _shaft_door_states.find(shaft_door); - if (it == _shaft_door_states.end()) - continue; - if (it->second && it->second->current_mode.value != door_mode) - publish_door_request(time, shaft_door, door_mode); - } -} - -void LiftCommon::open_doors(const double time) -{ - move_doors(time, DoorMode::MODE_OPEN); -} - -void LiftCommon::close_doors(const double time) -{ - move_doors(time, DoorMode::MODE_CLOSED); -} - -uint32_t LiftCommon::get_door_state( - const std::unordered_map>& floor_to_door_map, - const std::unordered_map& door_states) -{ - std::size_t open_count = 0; - std::size_t closed_count = 0; - const auto doors = floor_to_door_map.find( - _lift_state.current_floor)->second; - const std::size_t num = doors.size(); - for (const std::string& door : doors) - { - const auto& door_state = door_states.find(door)->second; - if ((door_state) && - (door_state->current_mode.value == DoorMode::MODE_CLOSED)) - closed_count++; - - else if ((door_state) && - (door_state->current_mode.value == DoorMode::MODE_OPEN)) - open_count++; - } - if (open_count == num) - return DoorMode::MODE_OPEN; - - else if (closed_count == num) - return DoorMode::MODE_CLOSED; - - else - return DoorMode::MODE_MOVING; -} - -void LiftCommon::update_lift_door_state() -{ - uint32_t cabin_door_state = get_door_state( - _floor_name_to_cabin_door_name, _cabin_door_states); - uint32_t shaft_door_state = get_door_state( - _floor_name_to_shaft_door_name, _shaft_door_states); - - _lift_state.door_state = (cabin_door_state == shaft_door_state) ? - cabin_door_state : LiftState::DOOR_MOVING; -} - -LiftCommon::LiftCommon(rclcpp::Node::SharedPtr node, - const std::string& lift_name, - const std::string& joint_name, - const MotionParams& cabin_motion_params, - const std::vector& floor_names, - const std::unordered_map& floor_name_to_elevation, - std::unordered_map< - std::string, std::vector> floor_name_to_shaft_door_name, - std::unordered_map< - std::string, std::vector> floor_name_to_cabin_door_name, - std::unordered_map shaft_door_states, - std::unordered_map cabin_door_states, - std::string initial_floor_name) -: _ros_node(node), - _lift_name(lift_name), - _cabin_joint_name(joint_name), - _cabin_motion_params(cabin_motion_params), - _floor_names(floor_names), - _floor_name_to_elevation(floor_name_to_elevation), - _floor_name_to_shaft_door_name(floor_name_to_shaft_door_name), - _floor_name_to_cabin_door_name(floor_name_to_cabin_door_name), - _shaft_door_states(shaft_door_states), - _cabin_door_states(cabin_door_states) -{ - // prints out available floors for this lift - std::cout << "Loaded lift: " << _lift_name << std::endl; - std::cout << "Names | Elevations" << std::endl; - for (const auto& it : _floor_name_to_elevation) - std::cout << it.first << " | " << it.second << std::endl; - - // initialize pub & sub - _lift_state_pub = _ros_node->create_publisher( - "/lift_states", rclcpp::SystemDefaultsQoS().keep_last(10)); - - _door_request_pub = _ros_node->create_publisher( - "/adapter_door_requests", rclcpp::SystemDefaultsQoS().keep_last(10)); - - _lift_request_sub = _ros_node->create_subscription( - "/lift_requests", rclcpp::SystemDefaultsQoS().keep_last(10), - [&](LiftRequest::UniquePtr msg) - { - if (msg->lift_name != _lift_name) - return; - - if (_floor_name_to_elevation.find( - msg->destination_floor) == _floor_name_to_elevation.end()) - { - RCLCPP_INFO(logger(), - "Received request for unavailable floor [%s]", - msg->destination_floor.c_str()); - return; - } - - // Trigger an error if a request, different from previous one, comes in - // Noop if request is the same - if (_lift_request) - { - if (_lift_request->destination_floor != msg->destination_floor || - _lift_request->request_type != msg->request_type || - _lift_request->session_id != msg->session_id) - { - RCLCPP_INFO(logger(), - "Discarding request: [%s] is busy at the moment", - _lift_name.c_str()); - } - return; - } - - _lift_request = std::move(msg); - RCLCPP_INFO(logger(), - "Lift [%s] requested at level [%s]", - _lift_name.c_str(), _lift_request->destination_floor.c_str()); - }); - - _door_state_sub = _ros_node->create_subscription( - "/door_states", rclcpp::SystemDefaultsQoS().keep_last(10), - [&](DoorState::SharedPtr msg) - { - std::string name = msg->door_name; - if (_cabin_door_states.find(name) != _cabin_door_states.end()) - _cabin_door_states[name] = std::move(msg); - else if (_shaft_door_states.find(name) != _shaft_door_states.end()) - _shaft_door_states[name] = std::move(msg); - }); - - // Initial lift state - _lift_state.lift_name = _lift_name; - _lift_state.current_floor = _floor_names[0]; - _lift_state.destination_floor = initial_floor_name; - _lift_state.door_state = LiftState::DOOR_CLOSED; - _lift_state.motion_state = LiftState::MOTION_STOPPED; - _lift_state.current_mode = LiftState::MODE_AGV; - for (const std::string& floor_name : _floor_names) - _lift_state.available_floors.push_back(floor_name); -} - -void LiftCommon::pub_lift_state(const double time) -{ - _last_pub_time = time; - const int32_t t_sec = static_cast(time); - const uint32_t t_nsec = - static_cast((time-static_cast(t_sec)) *1e9); - const rclcpp::Time now{t_sec, t_nsec, RCL_ROS_TIME}; - _lift_state.lift_time = now; - _lift_state_pub->publish(_lift_state); -} - -LiftCommon::LiftUpdateResult LiftCommon::update(const double time, - const double position, const double velocity) -{ - const double dt = time - _last_update_time; - _last_update_time = time; - - // Update lift state - update_cabin_state(position, velocity); - update_lift_door_state(); - - // Construct LiftUpdateResult - LiftCommon::LiftUpdateResult result; - result.velocity = 0.0; - result.fmax = _cabin_motion_params.f_max; - - // Handle lift request - if (_lift_request) - { - std::string desired_floor = _lift_request->destination_floor; - uint8_t desired_door_state = _lift_request->door_state; - if (_lift_request->request_type == LiftRequest::REQUEST_END_SESSION) - _lift_state.session_id = ""; - else - _lift_state.session_id = _lift_request->session_id; - - if ((_lift_state.current_floor == desired_floor) && - (_lift_state.door_state == desired_door_state) && - (_lift_state.motion_state == LiftState::MOTION_STOPPED)) - { - RCLCPP_INFO(logger(), - "Reached floor %s with doors %s", - desired_floor.c_str(), desired_door_state == 0 ? "closed" : "open"); - _lift_request = nullptr; - } - else - { - _lift_state.destination_floor = desired_floor; - - if (_lift_state.current_floor != _lift_state.destination_floor) - { - if (_lift_state.door_state != LiftState::DOOR_CLOSED) - { - close_doors(time); - } - else - { - result.velocity = get_step_velocity(dt, position, velocity); - } - } - else - { - if (_lift_state.motion_state != LiftState::MOTION_STOPPED) - { - result.velocity = get_step_velocity(dt, position, velocity); - } - else - { - if (desired_door_state == LiftState::DOOR_OPEN) - { - open_doors(time); - } - else if (desired_door_state == LiftState::DOOR_CLOSED) - { - close_doors(time); - } - } - } - } - } - // Publish lift state at 1 Hz - if (time - _last_pub_time >= 1.0) - pub_lift_state(time); - - return result; -} - -} // namespace rmf_building_sim_common diff --git a/rmf_building_sim_common/src/utils.cpp b/rmf_building_sim_common/src/utils.cpp deleted file mode 100644 index 4fea4a6..0000000 --- a/rmf_building_sim_common/src/utils.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include -#include -#include - -#include - -namespace rmf_building_sim_common { - -//============================================================================== -double compute_ds( - double s_target, - double v_actual, - const double v_max, - const double accel_nom, - const double accel_max, - const double dt) -{ - double sign = 1.0; - if (s_target < 0.0) - { - // Limits get confusing when we need to go backwards, so we'll flip signs - // here so that we pretend the target is forwards - s_target *= -1.0; - v_actual *= -1.0; - sign = -1.0; - } - - // We should try not to shoot past the targstd::vector connections;et - double next_s = s_target / dt; - - // Test velocity limit - next_s = std::min(next_s, v_max); - - // Test acceleration limit - next_s = std::min(next_s, accel_nom * dt + v_actual); - - if (v_actual > 0.0 && s_target > 0.0) - { - // This is what our deceleration should be if we want to begin a constant - // deceleration from now until we reach the goal - double deceleration = pow(v_actual, 2) / s_target; - deceleration = std::min(deceleration, accel_max); - - if (accel_nom <= deceleration) - { - // If the smallest constant deceleration for reaching the goal is - // greater than - next_s = -deceleration * dt + v_actual; - } - } - - // Flip the sign the to correct direction before returning the value - return sign * next_s; -} - -//============================================================================== -double compute_desired_rate_of_change( - double _s_target, - double _v_actual, - const MotionParams& _motion_params, - const double _dt) -{ - double sign = 1.0; - if (_s_target < 0.0) - { - // Limits get confusing when we need to go backwards, so we'll flip signs - // here so that we pretend the target is forwards - _s_target *= -1.0; - _v_actual *= -1.0; - sign = -1.0; - } - - // We should try not to shoot past the target - double v_next = _s_target / _dt; - - // Test velocity limit - v_next = std::min(v_next, _motion_params.v_max); - - // Test acceleration limit - v_next = std::min(v_next, _motion_params.a_nom * _dt + _v_actual); - - if (_v_actual > 0.0 && _s_target > 0.0) - { - // This is what our deceleration should be if we want to begin a constant - // deceleration from now until we reach the goal - double deceleration = pow(_v_actual, 2) / (2.0 * _s_target); - deceleration = std::min(deceleration, _motion_params.a_max); - - if (_motion_params.a_nom <= deceleration) - { - // If the smallest constant deceleration for reaching the goal is - // greater than the nominal acceleration, then we should begin - // decelerating right away so that we can smoothly reach the goal while - // decelerating as close to the nominal acceleration as possible. - v_next = -deceleration * _dt + _v_actual; - } - } - - // Flip the sign to the correct direction before returning the value - return sign * v_next; -} - -// ROS 2 nodes can only have alphanumeric characters and underscores, this -// function removes special characters and replaces dashes / spaces with -// underscores to avoid throwing exceptions. -//============================================================================== -void sanitize_node_name(std::string& node_name) -{ - std::replace(node_name.begin(), node_name.end(), ' ', '_'); - std::replace(node_name.begin(), node_name.end(), '-', '_'); - node_name.erase(std::remove_if(node_name.begin(), node_name.end(), - [](auto const& c) -> bool - { - return c != '_' && !std::isalnum(c); - }), node_name.end()); -} -} -// namespace rmf_building_sim_common diff --git a/rmf_building_sim_gz_classic_plugins/CHANGELOG.rst b/rmf_building_sim_gz_classic_plugins/CHANGELOG.rst deleted file mode 100644 index 168b6e2..0000000 --- a/rmf_building_sim_gz_classic_plugins/CHANGELOG.rst +++ /dev/null @@ -1,68 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rmf\_building\_sim\_gz\_classic\_plugins -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.2.2 (2023-12-20) ------------------- -* Sanitize node names to avoid plugin exceptions (`#110 `_) -* Contributors: Luca Della Vedova - -2.2.1 (2023-06-30) ------------------- - -2.2.0 (2023-06-08) ------------------- - -2.1.0 (2023-06-06) ------------------- -* Switch to rst changelogs (`#101 `_) -* Contributors: Yadunund - -2.0.0 (2022-10-04) ------------------- -* Renamed to rmf\_building\_sim\_gz\_classic\_plugins and Humble migration (`#77 `_) -* Contributors: Luca Della Vedova, Yadunund - -1.3.1 (2021-30-11) ------------------- -* Making install location package specific to fix (`#100 `_) (`#60 `_) -* Contributors: Marco A. Gutierrez - -1.3.0 (2021-09-01) ------------------- -* Fix dependencies (`#26 `_) -* fix compile error on qt \> 5.14 (`#27 `_) -* Changes for galactic (`#25 `_) -* Make menge mandatory and fix building\_sim dependencies (`#19 `_) -* Add quality declaration documents (`#1 `_) -* Crowd step size fix for large physics step sizes (`#10 `_) -* Slotcar plugin package move and utils cleanup (`#5 `_) -* account for renaming to rmf\_building\_map\_tools (`#4 `_) -* account for renaming from building\_map\_msgs to rmf\_building\_map\_msgs (`#3 `_) -* Contributors: Charayaphan Nakorn Boon Han, Geoffrey Biggs, Luca Della Vedova, Marco A. Gutiérrez, Teo Koon Peng, Yadunund - -1.2.0 (2021-01-06) ------------------- -* Merge branch \'release-1.1\' -* Control slotcar with model velocity cmds in place of joint velocity cmds (`#236 `_) -* Implement battery drain and recharge for slotcars (`#242 `_) -* Building\_crowdsim for generating the navmesh file and required configuration files for menge -* (`#224 `_) -* Add animation switch to crowd simulation plugin (`#238 `_) -* Contributors: Geoffrey Biggs, Guoliang (Fred) Shao, Marco A. Gutierrez, Marco A. Gutiérrez, Rushyendra Maganty - -1.1.0 (2020-09-24) ------------------- -* Implement model visibility toggling (`#226 `_) -* Add crowd simulation plugin (`#218 `_) -* Foxy support (`#194 `_) -* Modularization of doors and slotcar (`#138 `_) -* Add gazebo lift plugin -* Contributors: Charayaphan Nakorn Boon Han, Guoliang (Fred) Shao, Kevin\_Skywalker, Luca Della Vedova, MakinoharaShouko, Yadunund - -1.0.0 (2020-06-22) ------------------- -* changed to using target\_link\_libraries to be specific (`#95 `_) -* Add support for single doors (`#90 `_) -* Add gazebo plugins used by rmf\_building\_map\_tools generators (`#89 `_) -* Contributors: Aaron, Aaron Chong, Michael X. Grey, Morgan Quigley, Yadunund diff --git a/rmf_building_sim_gz_classic_plugins/CMakeLists.txt b/rmf_building_sim_gz_classic_plugins/CMakeLists.txt deleted file mode 100644 index 9ffa144..0000000 --- a/rmf_building_sim_gz_classic_plugins/CMakeLists.txt +++ /dev/null @@ -1,161 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rmf_building_sim_gz_classic_plugins) - -find_package(Qt5 COMPONENTS Widgets REQUIRED) -set(CMAKE_AUTOMOC ON) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(gazebo_dev REQUIRED) -find_package(gazebo_ros REQUIRED) -find_package(OpenCV REQUIRED ) -find_package(rmf_fleet_msgs REQUIRED) -find_package(rmf_building_sim_common REQUIRED) -find_package(menge_vendor REQUIRED) - -include(GNUInstallDirs) - -############################### -# door stuff -############################### - -add_library(door SHARED src/door.cpp) - -ament_target_dependencies(door - rmf_building_sim_common - rclcpp - gazebo_ros -) - -target_include_directories(door - PUBLIC - $ - $ - ${GAZEBO_INCLUDE_DIRS} -) - -############################### -# lift stuff -############################### - -add_library(lift SHARED src/lift.cpp) - -ament_target_dependencies(lift - rmf_building_sim_common - rclcpp - gazebo_ros -) - -target_include_directories(lift - PUBLIC - $ - $ - ${GAZEBO_INCLUDE_DIRS} -) - -############################### -# toggle floor stuff -############################### - -add_library(toggle_floors SHARED src/toggle_floors.cpp) - -ament_target_dependencies(toggle_floors - Qt5 - gazebo_ros - rmf_fleet_msgs - rclcpp -) - -target_include_directories(toggle_floors - PUBLIC - ${GAZEBO_INCLUDE_DIRS} - #${rmf_building_sim_common_INCLUDE_DIRS} - ${Qt5Core_INCLUDE_DIRS} -) - -############################### -# toggle charging -############################### - -add_library(toggle_charging SHARED src/toggle_charging.cpp) - -ament_target_dependencies(toggle_charging - Qt5 - gazebo_ros - rmf_fleet_msgs - rclcpp -) - -target_include_directories(toggle_charging - PUBLIC - ${GAZEBO_INCLUDE_DIRS} - ${Qt5Core_INCLUDE_DIRS} -) - - -############################### -# thumbnail generation stuff -############################### - -add_library(thumbnail_generator SHARED src/thumbnail_generator.cpp) - -target_include_directories(thumbnail_generator - PUBLIC - ${GAZEBO_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} -) - -target_link_libraries(thumbnail_generator - PUBLIC - ${OpenCV_LIBS} -) - -############################### -# crowd simulator stuff -############################### -add_library(crowd_simulator - SHARED - src/crowd_simulator.cpp -) - -ament_target_dependencies(crowd_simulator - rmf_building_sim_common - rclcpp - menge_vendor -) - -target_include_directories(crowd_simulator - PUBLIC - $ - ${GAZEBO_INCLUDE_DIRS} - ${menge_vendor_INCLUDE_DIRS} - ${rmf_building_sim_common_INCLUDE_DIRS} -) - -############################### -# install stuff -############################### - -install( - TARGETS door lift toggle_floors toggle_charging thumbnail_generator crowd_simulator - LIBRARY DESTINATION lib/${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/rmf_building_sim_gz_classic_plugins/QUALITY_DECLARATION.md b/rmf_building_sim_gz_classic_plugins/QUALITY_DECLARATION.md deleted file mode 100644 index cd6adff..0000000 --- a/rmf_building_sim_gz_classic_plugins/QUALITY_DECLARATION.md +++ /dev/null @@ -1,172 +0,0 @@ -This document is a declaration of software quality for the `rmf_building_sim_gz_classic_plugins` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). - -# `rmf_building_sim_gz_classic_plugins` Quality Declaration - -The package `rmf_building_sim_gz_classic_plugins` claims to be in the **Quality Level 4** category. - -Below are the detailed rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 4 in REP-2004](https://www.ros.org/reps/rep-2004.html). - -## Version Policy [1] - -### Version Scheme [1.i] - -`rmf_building_sim_gz_classic_plugins` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). - -### Version Stability [1.ii] - -`rmf_building_sim_gz_classic_plugins` is at a stable version, i.e. `>= 1.0.0`. -The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). - -### Public API Declaration [1.iii] - -`rmf_building_sim_gz_classic_plugins` does not have a public API. - -### API Stability Policy [1.iv] - -`rmf_building_sim_gz_classic_plugins` does not have a public API. - -### ABI Stability Policy [1.v] - -`rmf_building_sim_gz_classic_plugins` does not have a public API. - -### API and ABI Stability Within a Released ROS Distribution [1.vi] - -`rmf_building_sim_gz_classic_plugins` does not have a public API. - -## Change Control Process [2] - -`rmf_building_sim_gz_classic_plugins` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-requirements). - -### Change Requests [2.i] - -`rmf_building_sim_gz_classic_plugins` requires that all changes occur through a pull request. - -### Contributor Origin [2.ii] - -`rmf_building_sim_gz_classic_plugins` uses DCO as its confirmation of contributor origin policy. -More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). - -### Peer Review Policy [2.iii] - -All pull requests must have at least 1 peer review. - -### Continuous Integration [2.iv] - -All pull requests must pass CI on all platforms supported by RMF. -The CI checks only that the package builds. -The most recent CI results can be seen on [the workflow page](https://github.com/osrf/traffic_editor/actions). - -### Documentation Policy [2.v] - -All pull requests must resolve related documentation changes before merging. - -## Documentation [3] - -### Feature Documentation [3.i] - -`rmf_building_sim_gz_classic_plugins` does not provide feature documentation. - -### Public API Documentation [3.ii] - -`rmf_building_sim_gz_classic_plugins` does not have a public API. - -### License [3.iii] - -The license for `rmf_building_sim_gz_classic_plugins` is Apache 2.0, the type is declared in the [package.xml](package.xml) manifest file, and a full copy of the license is in the repository level [LICENSE](../LICENSE) file. - -### Copyright Statement [3.iv] - -The copyright holders each provide a statement of copyright in each source code file in `rmf_demo_tasks`. - -### Quality declaration document [3.v] - -This quality declaration is linked in the [README file](README.md). - -This quality declaration has not been externally peer-reviewed and is not registered on any Level 4 lists. - -## Testing [4] - -### Feature Testing [4.i] - -`rmf_building_sim_gz_classic_plugins` does not have any tests. - -### Public API Testing [4.ii] - -`rmf_building_sim_gz_classic_plugins` does not have a public API. - -### Coverage [4.iii] - -`rmf_building_sim_gz_classic_plugins` does not track coverage statistics. - -### Performance [4.iv] - -`rmf_building_sim_gz_classic_plugins` does not have performance tests. - -### Linters and Static Analysis [4.v] - -`rmf_building_sim_gz_classic_plugins` does not use the standard linters and static analysis tools for its CMake code to ensure it follows the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters). - -## Dependencies [5] - -### Direct Runtime ROS Dependencies [5.i] - -`rmf_building_sim_gz_classic_plugins` has the following direct runtime ROS dependencies. - -#### rclcpp - -`rclcpp` is [**Quality Level 1**](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). - -#### gazebo\_dev - -`gazebo_dev` does not declare a Quality Level. -It is assumed to be at least **Quality Level 4** based on its widespread use. - -#### gazebo\_ros - -`gazebo_ros` does not declare a Quality Level. -It is assumed to be at least **Quality Level 4** based on its widespread use. - -#### rmf\_fleet\_msgs - -`rmf_fleet_msgs` is [**Quality Level 3**](https://github.com/open-rmf/rmf_internal_msgs/blob/main/rmf_fleet_msgs/QUALITY_DECLARATION.md). - -#### rmf\_building\_sim\_common - -`rmf_building_sim_common` is [**Quality Level 4**](../rmf_building_sim_common/QUALITY_DECLARATION.md). - -### Optional Direct Runtime ROS Dependencies [5.ii] - -`rmf_building_sim_gz_classic_plugins` does not have any optional direct runtime ROS dependencies. - -### Direct Runtime non-ROS Dependency [5.iii] - -#### OpenCV - -`OpenCV` does not declare a quality level. -It is assumed to be **Quality Level 1** based on wide-spread use. - -#### menge\_vendor - -`menge_vendor` does not declare a quality level. -It is assumed to be **Quality Level 4**. - -#### qtbase5-dev - -`qt5base-dev` is widely-used third-party software for building graphical applications. -Due to its wide use, documentation, and testing, it is assumed to be **Quality Level 3**. - -#### libqt5-widgets - -`libqt5-widgets` is widely-used third-party software for building graphical applications. -Due to its wide use, documentation, and testing, it is assumed to be **Quality Level 3**. - -## Platform Support [6] - -`rmf_building_sim_gz_classic_plugins` does not support all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers). -`rmf_building_sim_gz_classic_plugins` supports ROS Eloquent and ROS Foxy. - -## Security [7] - -### Vulnerability Disclosure Policy [7.i] - -This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rmf_building_sim_gz_classic_plugins/README.md b/rmf_building_sim_gz_classic_plugins/README.md deleted file mode 100644 index 4dadf26..0000000 --- a/rmf_building_sim_gz_classic_plugins/README.md +++ /dev/null @@ -1,7 +0,0 @@ -# rmf\_building\_sim\_gz_classic\_plugins - -This package provides Gazebo-classic simulator plugins for allowing simulated building infrastructure to communicate with a ROS system. - -## Quality Declaration - -This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](./QUALITY_DECLARATION.md) for more details. diff --git a/rmf_building_sim_gz_classic_plugins/package.xml b/rmf_building_sim_gz_classic_plugins/package.xml deleted file mode 100644 index ca80237..0000000 --- a/rmf_building_sim_gz_classic_plugins/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - rmf_building_sim_gz_classic_plugins - 2.2.2 - - Gazebo plugins so that buildings generated by rmf_building_map_tools can - talk to the ROS 2-based systems in the rmf_traffic_ros2 package. - - Morgan Quigley - Michael Grey - Roselle Carmen - Aaron Chong - Brandon Ong - Kevin Ma - Rushyendra Maganty - Morgan Quigley - Marco A. Gutiérrez - Apache 2.0 - - ament_cmake - - rclcpp - gazebo_dev - gazebo_ros - libopencv-dev - rmf_fleet_msgs - rmf_building_sim_common - menge_vendor - libqt5-widgets - qtbase5-dev - - - ament_cmake - - - diff --git a/rmf_building_sim_gz_classic_plugins/src/crowd_simulator.cpp b/rmf_building_sim_gz_classic_plugins/src/crowd_simulator.cpp deleted file mode 100644 index f30c039..0000000 --- a/rmf_building_sim_gz_classic_plugins/src/crowd_simulator.cpp +++ /dev/null @@ -1,319 +0,0 @@ -/* - * Copyright (C) 2020 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 -#include - -#include - -#include "crowd_simulator.hpp" - -namespace crowd_simulation_gazebo { - -//============================================ -//WorldPlugin -void CrowdSimulatorPlugin::Load( - gazebo::physics::WorldPtr world, - sdf::ElementPtr sdf) -{ - _world = world; - - if (!_crowd_sim_interface->read_sdf(sdf)) - { - RCLCPP_ERROR(_crowd_sim_interface->logger(), - "Error loading crowd simulator plugin. Load params failed!"); - exit(EXIT_FAILURE); - } - - if (!_crowd_sim_interface->enabled()) - { - RCLCPP_WARN(_crowd_sim_interface->logger(), - "CrowdSim is not enabled"); - return; - } - - if (!_crowd_sim_interface->init_crowd_sim()) - { - RCLCPP_ERROR( - _crowd_sim_interface->logger(), - "Crowd simulation failed to initialize."); - exit(EXIT_FAILURE); - } - - if (!_spawn_agents_in_world()) - { - RCLCPP_ERROR( - _crowd_sim_interface->logger(), - "Crowd simulation failed to spawn agents in the world."); - exit(EXIT_FAILURE); - } - - _update_connection_ptr = gazebo::event::Events::ConnectWorldUpdateBegin( - [this](gazebo::common::UpdateInfo update_info) - { - _update(update_info); - } - ); -} - -//============================================ -void CrowdSimulatorPlugin::_update( - const gazebo::common::UpdateInfo& update_info) -{ - //first round do nothing, initialize time stamp - if (_last_sim_time == gazebo::common::Time::Zero) - { - _last_sim_time = update_info.simTime; - } - - if (!_initialized) - { - // not initizalied - _init_spawned_agents(); - return; - } - - auto delta_sim_time = (update_info.simTime - _last_sim_time).Double(); - if (delta_sim_time >= _crowd_sim_interface->get_sim_time_step()) - { - _last_sim_time = update_info.simTime; - _crowd_sim_interface->one_step_sim(delta_sim_time); - _update_all_objects(delta_sim_time); - } -} - -//============================================ -void CrowdSimulatorPlugin::_update_all_objects(double delta_sim_time) -{ - for (size_t id = 0; id < _objects_count; id++) - { - ObjectPtr obj_ptr = _crowd_sim_interface->get_object_by_id(id); - gazebo::physics::ModelPtr model_ptr = _world->ModelByName( - obj_ptr->model_name); - - //update external agents - if (obj_ptr->agent_ptr->_external) - { - ignition::math::Pose3d pose = model_ptr->WorldPose(); - _crowd_sim_interface->update_external_agent( - obj_ptr->agent_ptr, pose); - continue; - } - - //update internal agents - auto type_ptr = _crowd_sim_interface->_model_type_db_ptr->get( - obj_ptr->type_name); - _update_internal_object(delta_sim_time, obj_ptr, - model_ptr, type_ptr); - } -} - -//============================================ -void CrowdSimulatorPlugin::_update_internal_object( - double delta_sim_time, - const ObjectPtr object_ptr, - const gazebo::physics::ModelPtr model_ptr, - const crowd_simulator::ModelTypeDatabase::RecordPtr type_ptr) -{ - if (!object_ptr) - { - RCLCPP_ERROR( - _crowd_sim_interface->logger(), "Null objectPtr when update Object!"); - return; - } - if (!model_ptr) - { - RCLCPP_ERROR( - _crowd_sim_interface->logger(), "Null modelPtr when update Object!"); - return; - } - - //update pose from menge to gazebo - ignition::math::Pose3d pose = - _crowd_sim_interface->get_agent_pose( - object_ptr->agent_ptr, - delta_sim_time); - - gazebo::physics::ActorPtr actor_ptr = - boost::dynamic_pointer_cast(model_ptr); - - auto delta_dist_vector = pose.Pos() - actor_ptr->WorldPose().Pos(); - // might need future work on 3D case - // the center of human has a z_elevation, which will make the human keep walking even if he reached the target - delta_dist_vector.Z(0.0); - double delta_dist = delta_dist_vector.Length(); - - auto init_pose = type_ptr->pose; - ignition::math::Pose3d anim_pose( - init_pose.x(), init_pose.y(), init_pose.z(), - init_pose.pitch(), init_pose.roll(), init_pose.yaw()); - //update x and y coordinates - anim_pose.Pos().X(pose.Pos().X()); - anim_pose.Pos().Y(pose.Pos().Y()); - - AnimState next_state = object_ptr->get_next_state( - delta_dist < _crowd_sim_interface->get_switch_anim_distance_th() && - !type_ptr->idle_animation.empty()); - - auto traj_info = actor_ptr->CustomTrajectory(); - switch (next_state) - { - case AnimState::WALK: - actor_ptr->SetScriptTime( - actor_ptr->ScriptTime() + delta_dist / type_ptr->animation_speed); - anim_pose.Rot() = pose.Rot(); - if (object_ptr->current_state != next_state) - traj_info->type = type_ptr->animation; - break; - - case AnimState::IDLE: - actor_ptr->SetScriptTime( - actor_ptr->ScriptTime() + delta_sim_time); - anim_pose.Rot() = actor_ptr->WorldPose().Rot(); - if (object_ptr->current_state != next_state) - traj_info->type = type_ptr->idle_animation; - break; - } - object_ptr->current_state = next_state; - actor_ptr->SetWorldPose(anim_pose); -} - -//============================================ -void CrowdSimulatorPlugin::_init_spawned_agents() -{ - _objects_count = _crowd_sim_interface->get_num_objects(); - for (size_t id = 0; id < _objects_count; ++id) - { - ObjectPtr obj_ptr = _crowd_sim_interface->get_object_by_id(id); - // spawned agents are not fully loaded - if (!_world->ModelByName(obj_ptr->model_name)) - { - _initialized = false; - return; - } - // all agents are loaded, set internal actors as non-static model and set custom trajectory - // because only non-static model can interact with slotcars - if (!obj_ptr->is_external) - { - gazebo::physics::ModelPtr model_ptr = _world->ModelByName( - obj_ptr->model_name); - gazebo::physics::ActorPtr actor_ptr = - boost::dynamic_pointer_cast(model_ptr); - gazebo::physics::TrajectoryInfoPtr trajectory_info(new gazebo::physics:: - TrajectoryInfo()); //matches the actor skeleton - - crowd_simulator::ModelTypeDatabase::RecordPtr type_ptr = - _crowd_sim_interface->_model_type_db_ptr->get(obj_ptr->type_name); - trajectory_info->type = type_ptr->animation; - // set each keyframe duration as the sim_time_step - trajectory_info->duration = _crowd_sim_interface->get_sim_time_step(); - actor_ptr->SetCustomTrajectory(trajectory_info); - actor_ptr->SetStatic(false); - - //check actor has idle animation - for (auto idle_anim : _crowd_sim_interface->get_switch_anim_name()) - { - if (actor_ptr->SkeletonAnimations().find(idle_anim) != - actor_ptr->SkeletonAnimations().end()) - { - type_ptr->idle_animation = idle_anim; - break; - } - } - } - } - _initialized = true; - RCLCPP_INFO( - _crowd_sim_interface->logger(), - "Gazebo models all loaded! Start simulating..."); -} - -//============================================ -bool CrowdSimulatorPlugin::_spawn_agents_in_world() -{ - //create model in world for each internal agents - _objects_count = _crowd_sim_interface->get_num_objects(); - for (size_t id = 0; id < _objects_count; id++) - { - if (!_crowd_sim_interface->get_object_by_id(id)->is_external) - { - auto object_ptr = _crowd_sim_interface->get_object_by_id(id); - assert(object_ptr); - auto type_ptr = _crowd_sim_interface->_model_type_db_ptr->get( - object_ptr->type_name); - assert(type_ptr); - if (!_create_model(object_ptr->model_name, type_ptr, - object_ptr->agent_ptr) ) - { - RCLCPP_INFO(_crowd_sim_interface->logger(), - "Failed to insert model [%s] in world", - object_ptr->model_name.c_str()); - return false; - } - } - } - return true; -} - -//============================================ -bool CrowdSimulatorPlugin::_create_model( - const std::string& model_name, - const crowd_simulator::ModelTypeDatabase::RecordPtr model_type_ptr, - const crowd_simulator::AgentPtr agent_ptr) -{ - sdf::ElementPtr model_element(new sdf::Element()); - model_element->SetName("include"); - - sdf::ElementPtr name_element(new sdf::Element()); - name_element->SetName("name"); - name_element->AddValue("string", model_name, true); - model_element->InsertElement(name_element); - - sdf::ElementPtr uri_element(new sdf::Element()); - uri_element->SetName("uri"); - uri_element->AddValue("string", model_type_ptr->file_name, true); - model_element->InsertElement(uri_element); - - sdf::ElementPtr static_element(new sdf::Element()); - static_element->SetName("static"); - static_element->AddValue("string", "False", true); - model_element->InsertElement(static_element); - - sdf::ElementPtr pose_element(new sdf::Element()); - pose_element->SetName("pose"); - std::ostringstream oss; - oss << agent_ptr->_pos.x() << " " << agent_ptr->_pos.y() << " " << "0 0 0 0"; - pose_element->AddValue("pose", oss.str(), true); - model_element->InsertElement(pose_element); - - sdf::SDFPtr sdf(new sdf::SDF()); - sdf->Root(model_element); - - assert(sdf); - _world->InsertModelSDF(*sdf); - RCLCPP_INFO(_crowd_sim_interface->logger(), - "Insert actor for crowd simulator agent: [%s] at [%s].", - model_name.c_str(), - oss.str().c_str()); - return true; -} - -// insert the plugin -GZ_REGISTER_WORLD_PLUGIN(CrowdSimulatorPlugin) -} //namespace crowd_simulation_gazebo diff --git a/rmf_building_sim_gz_classic_plugins/src/crowd_simulator.hpp b/rmf_building_sim_gz_classic_plugins/src/crowd_simulator.hpp deleted file mode 100644 index be145ed..0000000 --- a/rmf_building_sim_gz_classic_plugins/src/crowd_simulator.hpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Copyright (C) 2020 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 RMF_BUILDING_SIM_COMMON__CROWD_SIMULATOR_GAZEBO_HPP -#define RMF_BUILDING_SIM_COMMON__CROWD_SIMULATOR_GAZEBO_HPP - -#include -#include - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - - -namespace crowd_simulation_gazebo { - -using ObjectPtr = crowd_simulator::CrowdSimInterface::ObjectPtr; -//================================================================ -/* -* class CrowdSimulatorPlugin -*/ - -class CrowdSimulatorPlugin : public gazebo::WorldPlugin -{ - using AnimState = crowd_simulator::CrowdSimInterface::AnimState; -public: - CrowdSimulatorPlugin() - : _crowd_sim_interface(std::make_shared()), - _initialized(false), - _objects_count(0) - {} - - void Load(gazebo::physics::WorldPtr world, sdf::ElementPtr sdf) override; - -private: - std::shared_ptr _crowd_sim_interface; - bool _initialized; - size_t _objects_count; - gazebo::physics::WorldPtr _world; - gazebo::event::ConnectionPtr _update_connection_ptr; - gazebo::common::Time _last_sim_time; - - bool _spawn_agents_in_world(); - void _init_spawned_agents(); - void _update(const gazebo::common::UpdateInfo& update_info); //Update trigger function - void _update_all_objects(double delta_sim_time); - void _update_internal_object( - double delta_sim_time, - const ObjectPtr object_ptr, - const gazebo::physics::ModelPtr model_ptr, - const crowd_simulator::ModelTypeDatabase::RecordPtr type_ptr); - bool _create_model( - const std::string& model_name, - const crowd_simulator::ModelTypeDatabase::RecordPtr model_type_ptr, - const crowd_simulator::AgentPtr agent_ptr); -}; - -} //namespace crowd_simulation_gazebo - -#endif // CROWD_SIMULATION_GAZEBO__CROWD_SIMULATOR_GAZEBO_HPP diff --git a/rmf_building_sim_gz_classic_plugins/src/door.cpp b/rmf_building_sim_gz_classic_plugins/src/door.cpp deleted file mode 100644 index d1c33b0..0000000 --- a/rmf_building_sim_gz_classic_plugins/src/door.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -using namespace rmf_building_sim_common; - -namespace rmf_building_sim_gz_classic_plugins { -//============================================================================== - -class DoorPlugin : public gazebo::ModelPlugin -{ -private: - gazebo::event::ConnectionPtr _update_connection; - gazebo::physics::ModelPtr _model; - std::unordered_map _joints; - - std::shared_ptr _door_common = nullptr; - - bool _initialized = false; - -public: - DoorPlugin() - { - // Do nothing - } - - void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override - { - std::string node_name = model->GetName() + "_node"; - sanitize_node_name(node_name); - auto _ros_node = gazebo_ros::Node::Get(sdf, node_name); - _model = model; - - RCLCPP_INFO( - _ros_node->get_logger(), - "Loading DoorPlugin for [%s]", - _model->GetName().c_str()); - - _door_common = DoorCommon::make( - _model->GetName(), - _ros_node, - sdf); - - if (!_door_common) - return; - - for (const auto& joint_name : _door_common->joint_names()) - { - const auto joint = _model->GetJoint(joint_name); - if (!joint) - { - RCLCPP_ERROR(_ros_node->get_logger(), - " -- Model is missing the joint [%s]", - joint_name.c_str()); - return; - } - _joints.insert(std::make_pair(joint_name, joint)); - } - - _initialized = true; - - _update_connection = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&DoorPlugin::on_update, this)); - - RCLCPP_INFO(_ros_node->get_logger(), - "Finished loading [%s]", - _model->GetName().c_str()); - } - -private: - - void on_update() - { - if (!_initialized) - return; - - const double t = _model->GetWorld()->SimTime().Double(); - - // Create DoorUpdateRequest - std::vector requests; - for (const auto& joint : _joints) - { - DoorCommon::DoorUpdateRequest request; - request.joint_name = joint.first; - request.position = joint.second->Position(0); - request.velocity = joint.second->GetVelocity(0); - requests.push_back(request); - } - - auto results = _door_common->update(t, requests); - - // Apply motions to the joints - for (const auto& result : results) - { - const auto it = _joints.find(result.joint_name); - assert(it != _joints.end()); - it->second->SetParam("vel", 0, result.velocity); - it->second->SetParam("fmax", 0, result.fmax); - } - } - -}; - -GZ_REGISTER_MODEL_PLUGIN(DoorPlugin) - -} // namespace rmf_building_sim_gz_classic_plugins diff --git a/rmf_building_sim_gz_classic_plugins/src/lift.cpp b/rmf_building_sim_gz_classic_plugins/src/lift.cpp deleted file mode 100644 index 2f159e3..0000000 --- a/rmf_building_sim_gz_classic_plugins/src/lift.cpp +++ /dev/null @@ -1,97 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -using namespace rmf_building_sim_common; - -namespace building_sim_gazebo { -//============================================================================== - -class LiftPlugin : public gazebo::ModelPlugin -{ -private: - // Gazebo items - gazebo::event::ConnectionPtr _update_connection; - gazebo::physics::ModelPtr _model; - gazebo::physics::JointPtr _cabin_joint_ptr; - gazebo_ros::Node::SharedPtr _ros_node; - - std::unique_ptr _lift_common = nullptr; - - bool _initialized; - -public: - LiftPlugin() - { - _initialized = false; - } - - void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override - { - std::string node_name = model->GetName() + "_node"; - sanitize_node_name(node_name); - _ros_node = gazebo_ros::Node::Get(sdf, node_name); - _model = model; - - RCLCPP_INFO(_ros_node->get_logger(), - "Loading LiftPlugin for [%s]", - _model->GetName().c_str()); - - // load Lift object - _lift_common = LiftCommon::make(_model->GetName(), _ros_node, sdf); - if (!_lift_common) - { - RCLCPP_ERROR(_ros_node->get_logger(), - "Failed when loading [%s]", - _model->GetName().c_str()); - return; - } - - _cabin_joint_ptr = _model->GetJoint(_lift_common->get_joint_name()); - if (!_cabin_joint_ptr) - { - RCLCPP_ERROR(_ros_node->get_logger(), - " -- Model is missing the joint [%s]", - _lift_common->get_joint_name().c_str()); - return; - } - - _update_connection = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&LiftPlugin::on_update, this)); - - _cabin_joint_ptr->SetPosition(0, _lift_common->get_elevation()); - - RCLCPP_INFO(_ros_node->get_logger(), - "Finished loading [%s]", - _model->GetName().c_str()); - - _initialized = true; - } - -private: - void on_update() - { - if (!_initialized) - return; - - const double t = _model->GetWorld()->SimTime().Double(); - const double position = _cabin_joint_ptr->Position(0); - const double velocity = _cabin_joint_ptr->GetVelocity(0); - - // Send update request - auto result = _lift_common->update(t, position, velocity); - - _cabin_joint_ptr->SetParam("vel", 0, result.velocity); - _cabin_joint_ptr->SetParam("fmax", 0, result.fmax); - } -}; - -GZ_REGISTER_MODEL_PLUGIN(LiftPlugin) - -} // namespace building_sim_gazebo diff --git a/rmf_building_sim_gz_classic_plugins/src/thumbnail_generator.cpp b/rmf_building_sim_gz_classic_plugins/src/thumbnail_generator.cpp deleted file mode 100644 index 584a92f..0000000 --- a/rmf_building_sim_gz_classic_plugins/src/thumbnail_generator.cpp +++ /dev/null @@ -1,317 +0,0 @@ -/* - * Copyright (C) 2020 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 -#include -#include -#include - -#include "gazebo/msgs/msgs.hh" -#include "gazebo/transport/transport.hh" -#include "gazebo/sensors/SensorsIface.hh" -#include "gazebo/common/Plugin.hh" -#include "gazebo/rendering/rendering.hh" -#include "gazebo/util/system.hh" - -using namespace gazebo; -namespace po = boost::program_options; - -class GZ_PLUGIN_VISIBLE ThumbnailGenerator : public SystemPlugin -{ - -public: - ///////////////////////////////////////////// - virtual ~ThumbnailGenerator() - { - rendering::fini(); - } - - ///////////////////////////////////////////// - void Load(int _argc, char** _argv) - { - // System arguments Parser - try - { - po::options_description option{"Options"}; - po::options_description config("Optional Configs"); - po::options_description all_options{"Allowed Options"}; - - option.add_options()("input", po::value(), - "Path of input sdf model")("output", po::value(), - "Path of output directory"); - - config.add_options()("img-size", po::value()->default_value(4000), - "Output thumbnail Image pixel size")("cam-height", - po::value()->default_value(200), - "Scene camara height")("cam-hfov", - po::value()->default_value(0.08), - "Scene camera horizontal FOV"); - - all_options.add(option).add(config); - _print_options_stream << all_options; - - po::variables_map vm; - po::store(po::command_line_parser(_argc, _argv).options( - all_options).allow_unregistered().run(), vm); - po::notify(vm); - - _model_path = vm["input"].as(); - _output_path = boost::filesystem::path(vm["output"].as()); - _img_size = vm["img-size"].as(); - _cam_height = vm["cam-height"].as(); - _cam_hfov = vm["cam-hfov"].as(); - - printf(" - input model: %s \n", _model_path.c_str()); - printf(" - output dir: %s \n", _output_path.c_str()); - printf(" - configs: %d, %f, %f \n", _img_size, _cam_height, _cam_hfov); - } - catch (boost::exception& _e) - { - std::cerr << "\nError: Invalid arguments\n"<PrintHelp(); - _exit_flag = true; - return; - } - - if (!boost::filesystem::exists(_output_path)) - boost::filesystem::create_directories(_output_path); - - std::ifstream ifs(_model_path.c_str()); - if (!ifs) - { - std::cerr << "Error: Unable to open file[" << _model_path << "]\n"; - this->PrintHelp(); - _exit_flag = true; - return; - } - - this->_sdf_model.reset(new sdf::SDF()); - if (!sdf::init(this->_sdf_model)) - { - std::cerr << "ERROR: SDF parsing the xml failed" << std::endl; - this->PrintHelp(); - _exit_flag = true; - return; - } - - if (!sdf::readFile(_model_path, this->_sdf_model)) - { - std::cerr << "Error: SDF parsing the xml failed\n"; - this->PrintHelp(); - _exit_flag = true; - return; - } - - sdf::ElementPtr model_elem = this->_sdf_model->Root()->GetElement("model"); - this->_model_name = model_elem->Get("name"); - _exit_flag = false; - } - - ///////////////////////////////////////////// - void Init() - { - gazebo::sensors::disable(); - - this->_world_created_conn = event::Events::ConnectWorldCreated( - std::bind(&ThumbnailGenerator::OnWorldCreated, this)); - - this->_update_conn = event::Events::ConnectWorldUpdateBegin( - std::bind(&ThumbnailGenerator::Update, this)); - - this->_transport_node = transport::NodePtr(new transport::Node()); - this->_transport_node->Init(); - this->_factory_pub = this->_transport_node->Advertise( - "~/factory"); - this->_server_control_pub = - this->_transport_node->Advertise( - "/gazebo/server/control"); - } - - ///////////////////////////////////////////// - void OnWorldCreated() - { - this->_factory_pub->WaitForConnection(); - - if (this->_sdf_model) - { - msgs::Factory msg; - msg.set_sdf(this->_sdf_model->ToString()); - this->_factory_pub->Publish(msg, true); - } - } - - ///////////////////////////////////////////// - void Update() - { - if (_exit_flag) - { - // Clean up Connections and Cameras - this->_world_created_conn.reset(); - this->_update_conn.reset(); - this->_camera.reset(); - - // Tell the server to stop. - msgs::ServerControl msg; - msg.set_stop(true); - this->_server_control_pub->Publish(msg); - printf(" Done, Exiting \n"); - return; - } - - // Make sure to initialize the rendering engine in the same thread that will - // capture images. - if (!this->_scene) - { - printf(" Initializing Thumbnails Gen Plugin \n"); - rendering::load(); - rendering::init(); - - sdf::ElementPtr cameraSDF(new sdf::Element); - sdf::initFile("camera.sdf", cameraSDF); - - this->_scene = rendering::create_scene("default", false, true); - this->_camera = this->_scene->CreateCamera("viewing_cam", true); - this->_camera->SetCaptureData(true); - this->_camera->Load(cameraSDF); - this->_camera->Init(); - this->_camera->SetHFOV(static_cast(_cam_hfov)); - this->_camera->SetImageWidth(_img_size); - this->_camera->SetImageHeight(_img_size); - this->_camera->CreateRenderTexture("RenderTex"); - this->_camera->SetClipDist(100, 1000); - - gazebo::rendering::RTShaderSystem::Instance()->UpdateShaders(); - return; - } - - // Main Thumbnail generation - if (this->_camera && this->_scene) - { - printf(" Generating Thumbnail for %s \n", this->_model_name.c_str()); - event::Events::preRender(); - - unsigned char* img_ptr; - cv::Mat mask; - - // Render scene with green background - this->_scene->SetAmbientColor(ignition::math::Color(1, 1, 1, 1)); - this->_scene->SetBackgroundColor(ignition::math::Color(0, 1, 0, 1)); - this->_scene->SetShadowsEnabled(false); - this->RenderCameraVisual(); - - // Create Mask acording from image with green background - // Get Image data from camera in scene - img_ptr = (unsigned char*)this->_camera->ImageData(); - cv::Mat green_img(cv::Size(_img_size, _img_size), CV_8UC3, img_ptr); - cv::inRange(green_img, cv::Scalar(0, 245, 0), - cv::Scalar(5, 255, 5), mask); - cv::bitwise_not(mask, mask); - - // Render scene with white background - // then apply "green" mask to white image to avoid green-fringe effects - this->_scene->SetAmbientColor(ignition::math::Color(1, 1, 1, 1)); - this->_scene->SetBackgroundColor(ignition::math::Color(1, 1, 1, 1)); - this->_scene->SetShadowsEnabled(false); - this->RenderCameraVisual(); - - // Get Image data from camera in scene - img_ptr = (unsigned char*)this->_camera->ImageData(); - cv::Mat white_img(cv::Size(_img_size, _img_size), CV_8UC3, img_ptr); - cv::cvtColor(white_img, white_img, cv::COLOR_BGR2RGB); - - // Created masked img with alpha val, then crop it! - cv::Mat masked_img(cv::Size(_img_size, _img_size), CV_8UC4); - cv::cvtColor(white_img, masked_img, cv::COLOR_RGB2RGBA); - std::vector channels; - cv::split(white_img, channels); - channels.push_back(mask); - cv::merge(channels, masked_img); - cv::Rect r = cv::boundingRect(channels[3]); - - // output thumbnail png - std::string img_name = this->_model_name + ".png"; - cv::imwrite((_output_path / img_name).string(), masked_img(r)); - std::cout << img_name << " saved!" << std::endl; - _exit_flag = true; - } - } - - ///////////////////////////////////////////// - void RenderCameraVisual() - { - rendering::VisualPtr vis = this->_scene->GetVisual(this->_model_name); - - // unfortunately, IGNITION_MATH_MAJOR_VERSION doesn't seem to be defined. - // we'll use the Gazebo version instead, since they usually (always?) - // move in lockstep -#if GAZEBO_MAJOR_VERSION <= 9 - ignition::math::Box bbox = vis->BoundingBox(); -#else - ignition::math::AxisAlignedBox bbox = vis->BoundingBox(); -#endif - - // Place the visual at the origin - ignition::math::Vector3d trans = bbox.Center(); - vis->SetWorldPose( - ignition::math::Pose3d(trans.X(), trans.Y(), trans.Z(), 0, 0, 0)); - bbox = vis->BoundingBox(); - - // Generate Top view PNG Img - ignition::math::Pose3d pose; - pose.Pos().Set(0, 0, _cam_height); - pose.Rot().Euler(0, IGN_DTOR(90), 0); - this->_camera->SetWorldPose(pose); - this->_camera->Update(); - this->_camera->Render(true); - this->_camera->PostRender(); - } - - ///////////////////////////////////////////// - void PrintHelp() - { - std::cout << "Usage: gzserver -s libthumbnail_gen.so empty.world " - << "[Options] [Optional Configs] \n" - << std::endl; - std::cout << _print_options_stream.str() << std::endl; - } - -private: - event::ConnectionPtr _update_conn; - event::ConnectionPtr _world_created_conn; - transport::NodePtr _transport_node; - transport::PublisherPtr _server_control_pub; - transport::PublisherPtr _factory_pub; - rendering::ScenePtr _scene; - rendering::CameraPtr _camera; - sdf::SDFPtr _sdf_model; - - bool _exit_flag; - std::stringstream _print_options_stream; - std::string _model_name; - std::string _model_path; - boost::filesystem::path _output_path; - - // Optional Configs - int _img_size; - double _cam_hfov; - double _cam_height; -}; - -// Register this plugin with the simulator -GZ_REGISTER_SYSTEM_PLUGIN(ThumbnailGenerator) diff --git a/rmf_building_sim_gz_classic_plugins/src/toggle_charging.cpp b/rmf_building_sim_gz_classic_plugins/src/toggle_charging.cpp deleted file mode 100644 index 1c82fe1..0000000 --- a/rmf_building_sim_gz_classic_plugins/src/toggle_charging.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/* - * Copyright (C) 2020 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 - -#ifndef Q_MOC_RUN -#include -#endif - -#include - -#include -#include - -#include -#include - -// GUI Plugin that creates buttons for enabling/disabling slotcar charging -// and publishes any change in state -class ToggleCharging : public gazebo::GUIPlugin -{ - Q_OBJECT - gazebo::transport::NodePtr _node; - gazebo::transport::PublisherPtr _charge_state_pub; - - bool _enable_charge = true; - bool _enable_instant_charge = false; - bool _enable_drain = true; - const std::string _enable_charge_str = "_enable_charge"; - const std::string _enable_instant_charge_str = "_enable_instant_charge"; - const std::string _enable_drain_str = "_enable_drain"; - -public: - ToggleCharging() - { - printf("ToggleCharging::ToggleCharging()\n"); - _node = gazebo::transport::NodePtr(new gazebo::transport::Node()); - _node->Init(); - _charge_state_pub = _node->Advertise( - "/charge_state"); - - QVBoxLayout* vbox = new QVBoxLayout; - - QPushButton* charge_button = create_button( - "Allow Charging", _enable_charge); - connect( - charge_button, - &QAbstractButton::clicked, - [this]() - { - this->_enable_charge = !this->_enable_charge; - this->button_clicked(_enable_charge_str, this->_enable_charge); - }); - vbox->addWidget(charge_button); - - QPushButton* instant_charge_button = create_button( - "Instant Charging", _enable_instant_charge); - connect( - instant_charge_button, - &QAbstractButton::clicked, - [this]() - { - this->_enable_instant_charge = !this->_enable_instant_charge; - this->button_clicked(_enable_instant_charge_str, - this->_enable_instant_charge); - }); - vbox->addWidget(instant_charge_button); - - QPushButton* drain_button = create_button( - "Allow Battery Drain", _enable_drain); - connect( - drain_button, - &QAbstractButton::clicked, - [this]() - { - this->_enable_drain = !this->_enable_drain; - this->button_clicked(_enable_drain_str, this->_enable_drain); - }); - vbox->addWidget(drain_button); - - setLayout(vbox); - this->move(0, 80); - } - - virtual ~ToggleCharging() - { - } - - void button_clicked(const std::string& name, bool selected) - { - gazebo::msgs::Selection charge_state_msg; - charge_state_msg.set_name(name); - charge_state_msg.set_selected(selected); - charge_state_msg.set_id(1); // id not necessary for current use case - _charge_state_pub->Publish(charge_state_msg); - } - -private: - QPushButton* create_button(const std::string& button_nm, bool init_val) - { - auto new_btn = new QPushButton(QString::fromStdString(button_nm)); - new_btn->setCheckable(true); - new_btn->setChecked(init_val); - return new_btn; - } -}; - -#include "toggle_charging.moc" - -GZ_REGISTER_GUI_PLUGIN(ToggleCharging) diff --git a/rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp b/rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp deleted file mode 100644 index d2260a6..0000000 --- a/rmf_building_sim_gz_classic_plugins/src/toggle_floors.cpp +++ /dev/null @@ -1,139 +0,0 @@ -#include -#include -#include - -#ifndef Q_MOC_RUN -#include -#include -#endif - -#include - -#include -#include -#include -#include - -#include -#include - -using std::string; -using FleetState = rmf_fleet_msgs::msg::FleetState; -using RobotState = rmf_fleet_msgs::msg::RobotState; - -class ToggleFloors : public gazebo::GUIPlugin -{ - Q_OBJECT - gazebo::transport::NodePtr node; - gazebo::transport::PublisherPtr visual_pub; - std::unordered_map> floor_visibility; - gazebo_ros::Node::SharedPtr ros_node; - rclcpp::Subscription::SharedPtr fleet_state_sub; - -public: - ToggleFloors() - : GUIPlugin() - { - printf("ToggleFloors::ToggleFloors()\n"); - node = gazebo::transport::NodePtr(new gazebo::transport::Node()); - node->Init(); - visual_pub = node->Advertise("~/visual"); - } - - virtual ~ToggleFloors() - { - } - - void Load(sdf::ElementPtr sdf) - { - printf("ToggleFloors::Load()\n"); - ros_node = gazebo_ros::Node::Get(sdf); - - // toggle non-static robots - fleet_state_sub = ros_node->create_subscription( - "/fleet_states", rclcpp::SystemDefaultsQoS().keep_last(10), - [&](FleetState::UniquePtr msg) - { - bool visible; - gazebo::msgs::Visual visual_msg; - visual_msg.set_parent_name("world"); - for (const RobotState& robot : msg->robots) - { - visible = floor_visibility[robot.location.level_name]; - visual_msg.set_name(robot.name); - visual_msg.set_visible(visible); - visual_pub->Publish(visual_msg); - } - }); - - QHBoxLayout* hbox = new QHBoxLayout; - - for (sdf::ElementPtr floor_ele = sdf->GetFirstElement(); - floor_ele; - floor_ele = floor_ele->GetNextElement("floor")) - { - if (floor_ele->GetName() != string("floor")) - continue; - string floor_name = floor_ele->GetAttribute("name")->GetAsString(); - string model_name = - floor_ele->GetAttribute("model_name")->GetAsString(); - - std::vector models; - auto model_ele = floor_ele->GetElement("model"); - while (model_ele) - { - if (model_ele->HasAttribute("name")) - models.push_back(model_ele->GetAttribute("name")->GetAsString()); - model_ele = model_ele->GetNextElement("model"); - } - floor_visibility[floor_name] = true; - - printf( - "ToggleFloors::Load found a floor element: [%s]->[%s]\n", - floor_name.c_str(), - model_name.c_str()); - - QPushButton* button = - new QPushButton(QString::fromStdString(floor_name)); - button->setCheckable(true); - button->setChecked(true); - connect( - button, - &QAbstractButton::clicked, - [this, button, floor_name, model_name, models]() - { - this->button_clicked(button, floor_name, model_name, models); - }); - hbox->addWidget(button); - } - setLayout(hbox); - } - - void button_clicked( - QPushButton* button, - string floor_name, - string model_name, - std::vector models) - { - bool visible = button->isChecked(); - floor_visibility[floor_name] = visible; - printf( - "clicked: [%s] %s\n", - model_name.c_str(), - visible ? "SHOW" : "HIDE"); - gazebo::msgs::Visual visual_msg; - visual_msg.set_parent_name("world"); - visual_msg.set_name(model_name); - visual_msg.set_visible(visible); - visual_pub->Publish(visual_msg); - for (const string& model : models) - { - visual_msg.set_name(model); - visual_pub->Publish(visual_msg); - } - } -}; - -#include "toggle_floors.moc" - -GZ_REGISTER_GUI_PLUGIN(ToggleFloors) diff --git a/rmf_building_sim_gz_plugins/CMakeLists.txt b/rmf_building_sim_gz_plugins/CMakeLists.txt index 1a3e1a3..ab1bdbb 100644 --- a/rmf_building_sim_gz_plugins/CMakeLists.txt +++ b/rmf_building_sim_gz_plugins/CMakeLists.txt @@ -18,33 +18,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies +find_package(rmf_door_msgs REQUIRED) +find_package(rmf_lift_msgs REQUIRED) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(ignition-cmake2 REQUIRED) - -ign_find_package(ignition-gazebo6 REQUIRED) -set(IGN_GAZEBO_VER 6) -ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) -set(IGN_PLUGIN_VER 1) -ign_find_package(ignition-common4 REQUIRED) -set(IGN_COMMON_VER 4) -ign_find_package(ignition-math6 REQUIRED) -set(IGN_MATH_VER 6) -ign_find_package(ignition-gui6 REQUIRED) -set(IGN_GUI_VER 6) -ign_find_package(ignition-msgs8 REQUIRED) -set(IGN_MSGS_VER 8) -ign_find_package(ignition-transport11 REQUIRED) -set(IGN_TRANSPORT_VER 11) -ign_find_package(ignition-rendering6 REQUIRED) -set(IGN_RENDERING_VER 6) -ign_find_package(sdformat12 REQUIRED) - - -find_package(rmf_building_sim_common REQUIRED) +find_package(gz_sim_vendor REQUIRED) +find_package(gz-sim REQUIRED) +find_package(gz_plugin_vendor REQUIRED) +find_package(gz-plugin REQUIRED) +find_package(gz_gui_vendor REQUIRED) +find_package(gz-gui REQUIRED) +find_package(gz_msgs_vendor REQUIRED) +find_package(gz-msgs REQUIRED) +find_package(gz_transport_vendor REQUIRED) +find_package(gz-transport REQUIRED) +find_package(gz_rendering_vendor REQUIRED) +find_package(gz-rendering REQUIRED) + find_package(rmf_fleet_msgs REQUIRED) -find_package (Qt5 +find_package(Qt5 COMPONENTS Core Qml @@ -63,17 +56,34 @@ add_library(door SHARED src/door.cpp) target_include_directories(door PUBLIC - ${rmf_building_sim_common_INCLUDE_DIRS} - ${geometry_msgs_INCLUDE_DIRS} ${rmf_door_msgs_INCLUDE_DIRS} + $ +) + +target_link_libraries(door + gz-sim::core + gz-plugin::core ) ament_target_dependencies(door - PUBLIC rclcpp - ignition-gazebo${IGN_GAZEBO_VER} - ignition-plugin${IGN_PLUGIN_VER} - rmf_building_sim_common + rmf_door_msgs +) + +############################### +# register_component stuff +############################### + +add_library(register_component SHARED src/register_component.cpp) + +target_include_directories(register_component + PUBLIC + $ +) + +target_link_libraries(register_component + gz-sim::core + gz-plugin::core ) ############################### @@ -84,16 +94,20 @@ add_library(lift SHARED src/lift.cpp) target_include_directories(lift PUBLIC - ${rmf_building_sim_common_INCLUDE_DIRS} - ${geometry_msgs_INCLUDE_DIRS} + ${rmf_lift_msgs_INCLUDE_DIRS} + ${rmf_door_msgs_INCLUDE_DIRS} + $ +) + +target_link_libraries(lift + gz-sim::core + gz-plugin::core ) ament_target_dependencies(lift - PUBLIC - rclcpp - ignition-gazebo${IGN_GAZEBO_VER} - ignition-plugin${IGN_PLUGIN_VER} - rmf_building_sim_common + rclcpp + rmf_lift_msgs + rmf_door_msgs ) ############################### @@ -107,17 +121,17 @@ add_library(crowd_simulator target_include_directories(crowd_simulator PUBLIC $ - ${rmf_building_sim_common_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ${menge_vendor_INCLUDE_DIRS} - ${IGNITION-COMMON_INCLUDE_DIRS} +) + +target_link_libraries(crowd_simulator + gz-sim::core + gz-plugin::core ) ament_target_dependencies(crowd_simulator - ignition-gazebo${IGN_GAZEBO_VER} - ignition-plugin${IGN_PLUGIN_VER} rclcpp - rmf_building_sim_common menge_vendor ) @@ -132,10 +146,13 @@ add_library(toggle_charging SHARED ${headers_MOC} ${resources_RCC} ) +target_link_libraries(toggle_charging + gz-sim::core + gz-msgs::core + gz-transport::core +) + ament_target_dependencies(toggle_charging - ignition-gui${IGN_GUI_VER} - ignition-msgs${IGN_MSGS_VER} - ignition-transport${IGN_TRANSPORT_VER} Qt5Core Qt5Qml Qt5Quick @@ -160,13 +177,16 @@ add_library(toggle_floors SHARED ${headers_MOC} ${resources_RCC} ) +target_link_libraries(toggle_floors + gz-sim::core + gz-gui::core + gz-msgs::core + gz-plugin::core + gz-rendering::core + gz-transport::core +) + ament_target_dependencies(toggle_floors - ignition-gazebo${IGN_GAZEBO_VER} - ignition-gui${IGN_GUI_VER} - ignition-msgs${IGN_MSGS_VER} - ignition-plugin${IGN_PLUGIN_VER} - ignition-rendering${IGN_RENDERING_VER} - ignition-transport${IGN_TRANSPORT_VER} Qt5Core Qt5Qml Qt5Quick @@ -185,10 +205,17 @@ target_include_directories(toggle_floors # install stuff ############################### +ament_export_include_directories(include) + install( - TARGETS door lift crowd_simulator toggle_charging toggle_floors + TARGETS door lift crowd_simulator toggle_charging toggle_floors register_component LIBRARY DESTINATION lib/${PROJECT_NAME} ARCHIVE DESTINATION lib/${PROJECT_NAME} ) +install( + DIRECTORY include/ + DESTINATION include +) + ament_package() diff --git a/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/components/Door.hpp b/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/components/Door.hpp new file mode 100644 index 0000000..4de41c8 --- /dev/null +++ b/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/components/Door.hpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2022 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 RMF_BUILDING_SIM_GZ_PLUGINS_COMPONENTS_DOOR_HPP +#define RMF_BUILDING_SIM_GZ_PLUGINS_COMPONENTS_DOOR_HPP + +#include +#include +#include + +#include + +namespace gz { +namespace sim { +struct DoorJoint +{ + std::string name; + double closed_position; + double open_position; +}; + +struct DoorData +{ + rmf_building_sim_gz_plugins::MotionParams params; + std::vector joints; + bool ros_interface; // Whether it's managed by RMF, false for lift doors +}; + +enum class DoorModeCmp +{ + CLOSE = 0, + MOVING = 1, + OPEN = 2 +}; + +namespace components { +/// \brief A component used to describe an RMF door. +using Door = Component; +GZ_SIM_REGISTER_COMPONENT("rmf_components.Door", Door) + +/// \brief A component used to command an RMF door to open / close. +using DoorCmd = Component; +GZ_SIM_REGISTER_COMPONENT("rmf_components.DoorCmd", DoorCmd) + +/// \brief A component used to show the state of an RMF door. +using DoorStateComp = Component; +GZ_SIM_REGISTER_COMPONENT("rmf_components.DoorStateComp", DoorStateComp) +} +} +} +#endif diff --git a/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/components/Lift.hpp b/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/components/Lift.hpp new file mode 100644 index 0000000..a68a2b8 --- /dev/null +++ b/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/components/Lift.hpp @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2022 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 RMF_BUILDING_SIM_GZ_PLUGINS_COMPONENTS_LIFT_HPP +#define RMF_BUILDING_SIM_GZ_PLUGINS_COMPONENTS_LIFT_HPP + +#include +#include +#include + + +#include +#include + +namespace gz { +namespace sim { +struct FloorDoorPair +{ + std::string cabin_door; + std::string shaft_door; +}; + +struct Floor +{ + double elevation; + std::vector doors; +}; + +struct LiftData +{ + std::string name; + std::unordered_map floors; // Maps name to floor + std::string initial_floor; + rmf_building_sim_gz_plugins::MotionParams params; + std::string cabin_joint; +}; + +struct LiftCommand +{ + uint8_t request_type; + std::string destination_floor; + std::string session_id; + DoorModeCmp door_state; +}; + +namespace components { +/// \brief A component used to describe an RMF lift. +using Lift = Component; +GZ_SIM_REGISTER_COMPONENT("rmf_components.Lift", Lift) + +/// \brief A component used to command an RMF lift to open / close. +using LiftCmd = Component; +GZ_SIM_REGISTER_COMPONENT("rmf_components.LiftCmd", LiftCmd) +} +} +} +#endif diff --git a/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/utils.hpp b/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/utils.hpp new file mode 100644 index 0000000..a84cf6f --- /dev/null +++ b/rmf_building_sim_gz_plugins/include/rmf_building_sim_gz_plugins/utils.hpp @@ -0,0 +1,72 @@ +#ifndef RMF_BUILDING_SIM_GZ_PLUGINS_UTILS_HPP +#define RMF_BUILDING_SIM_GZ_PLUGINS_UTILS_HPP + +#include + +namespace rmf_building_sim_gz_plugins { + +struct MotionParams +{ + double v_max = 0.2; + double a_max = 0.1; + double a_nom = 0.08; + double dx_min = 0.01; + double f_max = 10000000.0; +}; + +// TODO(luca) move this to a cpp file +double compute_desired_rate_of_change( + double _s_target, + double _v_actual, + const MotionParams& _motion_params, + const double _dt) +{ + double sign = 1.0; + if (_s_target < 0.0) + { + // Limits get confusing when we need to go backwards, so we'll flip signs + // here so that we pretend the target is forwards + _s_target *= -1.0; + _v_actual *= -1.0; + sign = -1.0; + } + + // We should try not to shoot past the target + double v_next = _s_target / _dt; + + // Test velocity limit + v_next = std::min(v_next, _motion_params.v_max); + + // Test acceleration limit + v_next = std::min(v_next, _motion_params.a_nom * _dt + _v_actual); + + if (_v_actual > 0.0 && _s_target > 0.0) + { + // This is what our deceleration should be if we want to begin a constant + // deceleration from now until we reach the goal + double deceleration = pow(_v_actual, 2) / (2.0 * _s_target); + deceleration = std::min(deceleration, _motion_params.a_max); + + if (_motion_params.a_nom <= deceleration) + { + // If the smallest constant deceleration for reaching the goal is + // greater than the nominal acceleration, then we should begin + // decelerating right away so that we can smoothly reach the goal while + // decelerating as close to the nominal acceleration as possible. + v_next = -deceleration * _dt + _v_actual; + } + } + + // Flip the sign to the correct direction before returning the value + return sign * v_next; +} + +/// Convert a duration into a double value representing seconds. +inline double to_seconds(const std::chrono::nanoseconds delta_t) +{ + using Sec64 = std::chrono::duration; + return std::chrono::duration_cast(delta_t).count(); +} + +} +#endif diff --git a/rmf_building_sim_gz_plugins/package.xml b/rmf_building_sim_gz_plugins/package.xml index 47e1e4b..64f3e8f 100644 --- a/rmf_building_sim_gz_plugins/package.xml +++ b/rmf_building_sim_gz_plugins/package.xml @@ -15,13 +15,19 @@ ament_cmake rclcpp - rmf_building_sim_common + rmf_door_msgs + rmf_lift_msgs rmf_fleet_msgs libqt5-core libqt5-qml libqt5-quick menge_vendor - ignition-fortress + gz_gui_vendor + gz_msgs_vendor + gz_plugin_vendor + gz_rendering_vendor + gz_sim_vendor + gz_transport_vendor ament_cmake diff --git a/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp b/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp index b654213..7ca99cf 100644 --- a/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp +++ b/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp @@ -20,22 +20,199 @@ #include -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "crowd_simulator.hpp" -using namespace ignition::gazebo; +using namespace gz::sim; -namespace crowd_simulation_ign { +namespace crowd_simulation_gz { + +static bool _load_model_init_pose( + const std::shared_ptr& model_type_element, + AgentPose3d& result) +{ + std::string pose_str; + if (model_type_element->template Get( + "initial_pose", pose_str, "")) + { + std::regex ws_re("\\s+"); //whitespace + std::vector parts( + std::sregex_token_iterator(pose_str.begin(), pose_str.end(), ws_re, -1), + std::sregex_token_iterator()); + + if (parts.size() != 6) + { + gzerr << + "Error loading in , 6 floats (x, y, z, pitch, roll, yaw) expected." + << std::endl; + return false; + } + + result.x(std::stod(parts[0]) ); + result.y(std::stod(parts[1]) ); + result.z(std::stod(parts[2]) ); + result.pitch(std::stod(parts[3]) ); + result.roll(std::stod(parts[4]) ); + result.yaw(std::stod(parts[5]) ); + } + return true; +} + +static gz::math::Pose3d get_agent_pose( + const AgentPtr agent_ptr, double delta_sim_time) +{ + //calculate future position in delta_sim_time. currently in 2d + double px = static_cast(agent_ptr->_pos.x()) + + static_cast(agent_ptr->_vel.x()) * delta_sim_time; + double py = static_cast(agent_ptr->_pos.y()) + + static_cast(agent_ptr->_vel.y()) * delta_sim_time; + + double x_rot = static_cast(agent_ptr->_orient.x()); + double y_rot = static_cast(agent_ptr->_orient.y()); + + return gz::math::Pose3d(px, py, 0, 0, 0, std::atan2(y_rot, x_rot)); +} //================================================= +bool CrowdSimulatorPlugin::read_sdf( + const std::shared_ptr& sdf) +{ + char* menge_resource_path = getenv("MENGE_RESOURCE_PATH"); + + if (menge_resource_path == nullptr || + strcmp(menge_resource_path, "") == 0) + { + gzwarn << + "MENGE_RESOURCE_PATH env is empty. Crowd simulation is disabled." << + std::endl; + return true; + } + + _enabled = true; + _resource_path = std::string(menge_resource_path); + gzmsg << "Crowd Sim is enabled! is : " << + _resource_path << std::endl; + + if (!sdf->HasElement("behavior_file")) + { + gzerr << "No behavior file found! Required!" << std::endl; + return false; + } + _behavior_file = + sdf->GetElementImpl("behavior_file")->template Get(); + + if (!sdf->HasElement("scene_file")) + { + gzerr << "No scene file found! Required!" << std::endl; + return false; + } + _scene_file = + sdf->GetElementImpl("scene_file")->template Get(); + + if (!sdf->HasElement("update_time_step")) + { + gzerr << "No update_time_step found! Required!" << + std::endl; + return false; + } + _sim_time_step = + sdf->GetElementImpl("update_time_step")->template Get(); + + if (!sdf->HasElement("model_type")) + { + gzerr << "No model type for agents found! element Required!" << + std::endl; + return false; + } + auto model_type_element = sdf->GetElementImpl("model_type"); + while (model_type_element) + { + std::string s; + if (!model_type_element->template Get("typename", s, "")) + { + gzerr << + "No model type name configured in ! Required" + << std::endl; + return false; + } + + auto model_type_ptr = _model_type_db_ptr.emplace(s, + std::make_shared() ); //unordered_map + model_type_ptr->type_name = s; + + if (!model_type_element->template Get("filename", + model_type_ptr->file_name, "")) + { + gzerr << + "No actor skin configured in ! Required" << + std::endl; + return false; + } + + if (!model_type_element->template Get("animation", + model_type_ptr->animation, "")) + { + gzerr << + "No animation configured in ! Required" << + std::endl; + return false; + } + + if (!model_type_element->template Get("animation_speed", + model_type_ptr->animation_speed, 0.0)) + { + gzerr << + "No animation speed configured in ! Required" + << std::endl; + return false; + } + + if (!model_type_element->HasElement("initial_pose")) + { + gzerr << + "No model initial pose configured in ! Required [" + << s << "]" << std::endl; + return false; + } + if (!_load_model_init_pose(model_type_element, model_type_ptr->pose)) + { + gzerr << + "Error loading model initial pose in ! Check in [" + << s << "]" << std::endl; + return false; + } + + model_type_element = model_type_element->GetNextElement( + "model_type"); + } + + if (!sdf->HasElement("external_agent")) + { + gzwarn << + "No external agent provided. is needed with a unique name defined above." + << std::endl; + } + auto external_agent_element = sdf->GetElementImpl("external_agent"); + while (external_agent_element) + { + auto ex_agent_name = external_agent_element->template Get(); + gzmsg << "Added external agent: [" << ex_agent_name << "]." << std::endl; + _external_agents.emplace_back(ex_agent_name); //just store the name + external_agent_element = external_agent_element->GetNextElement( + "external_agent"); + } + + return true; +} + void CrowdSimulatorPlugin::Configure( const Entity& entity, const std::shared_ptr& sdf, @@ -43,37 +220,62 @@ void CrowdSimulatorPlugin::Configure( EventManager& /*event_mgr*/) { _world = std::make_shared(entity); - RCLCPP_INFO(_crowd_sim_interface->logger(), - "Initializing world plugin with name: %s", - _world->Name(ecm).c_str()); + gzmsg << "Initializing world plugin with name: " << + _world->Name(ecm).c_str() << std::endl; _world_name = _world->Name(ecm); - if (!_crowd_sim_interface->read_sdf(sdf)) + if (!read_sdf(sdf)) { - RCLCPP_ERROR(_crowd_sim_interface->logger(), - "Error loading crowd simulator plugin. Load params failed!"); + gzerr << "Error loading crowd simulator plugin. Load params failed!" << + std::endl; exit(EXIT_FAILURE); } - if (!_crowd_sim_interface->enabled()) + if (!_enabled) { - RCLCPP_WARN(_crowd_sim_interface->logger(), - "CrowdSim is Disabled!"); + gzmsg << "CrowdSim is Disabled!" << std::endl; return; } - if (!_crowd_sim_interface->init_crowd_sim()) + _menge_handle = MengeHandle::init_and_make( + _resource_path, + _behavior_file, + _scene_file, + _sim_time_step); + + //_spawn_object(); + + //bool CrowdSimInterface::_spawn_object() { - RCLCPP_ERROR(_crowd_sim_interface->logger(), - "Error loading crowd simulator plugin. Load [ Menge ] failed!"); - exit(EXIT_FAILURE); + //External models are loaded first in scene file + size_t external_count = _external_agents.size(); + size_t total_agent_count = _menge_handle->get_agent_count(); + + for (size_t i = 0; i < external_count; ++i) + { + auto agent_ptr = _menge_handle->get_agent(i); + agent_ptr->_external = true; + _objects.emplace_back( + new Object{agent_ptr, _external_agents[i], agent_ptr->_typeName, true, + AnimState::WALK}); + } + + for (size_t i = external_count; i < total_agent_count; ++i) + { + auto agent_ptr = _menge_handle->get_agent(i); + agent_ptr->_external = false; + std::string model_name = "agent" + std::to_string(i); + _objects.emplace_back( + new Object{agent_ptr, model_name, agent_ptr->_typeName, false, + AnimState::WALK}); + } } if (!_spawn_agents_in_world()) { - RCLCPP_ERROR( - _crowd_sim_interface->logger(), - "Error loading crowd simulator plugin. Crowd Simulator failed to spawn agents in the world!"); + gzerr << + "Error loading crowd simulator plugin. Crowd Simulator failed to spawn agents in the world!" + << std::endl; exit(EXIT_FAILURE); } @@ -85,10 +287,10 @@ void CrowdSimulatorPlugin::PreUpdate( EntityComponentManager& ecm) { // check if crowd sim is enabled - if (!_crowd_sim_interface->enabled()) + if (!_enabled) return; - // wait for all the models and actors loaded in ignition rendering + // wait for all the models and actors loaded in gz rendering if (!_initialized) { _init_spawned_agents(ecm); @@ -99,37 +301,35 @@ void CrowdSimulatorPlugin::PreUpdate( if (info.paused) return; - // Note, the update_time_step parameter is ignored in ignition + // Note, the update_time_step parameter is ignored in gz // through GPU animated actors the performance is good enough that // we can afford to update at every iteration and have smooth animations std::chrono::duration delta_sim_time_tmp = info.simTime - _last_sim_time; double delta_sim_time = delta_sim_time_tmp.count(); _last_sim_time = info.simTime; - _crowd_sim_interface->one_step_sim(delta_sim_time); + _menge_handle->set_sim_time_step(delta_sim_time); + _menge_handle->sim_step(); _update_all_objects(delta_sim_time, ecm); } //========================================================== bool CrowdSimulatorPlugin::_spawn_agents_in_world() { - size_t object_count = this->_crowd_sim_interface->get_num_objects(); - for (size_t id = 0; id < object_count; ++id) + for (size_t id = 0; id < _objects.size(); ++id) { - auto object_ptr = this->_crowd_sim_interface->get_object_by_id(id); + auto object_ptr = _objects[id]; assert(object_ptr); _object_dic[object_ptr->model_name] = id; if (!object_ptr->is_external) { - auto type_ptr = _crowd_sim_interface->_model_type_db_ptr->get( - object_ptr->type_name); + auto type_ptr = _model_type_db_ptr.get(object_ptr->type_name); assert(type_ptr); if (!this->_create_entity(object_ptr->model_name, type_ptr) ) { - RCLCPP_ERROR(_crowd_sim_interface->logger(), - "Failed to insert model [ %s ] in world", - object_ptr->model_name.c_str()); + gzerr << "Failed to insert model [ " << object_ptr->model_name << + " ] in world" << std::endl; return false; } } @@ -143,10 +343,9 @@ void CrowdSimulatorPlugin::_init_spawned_agents( { // check all the models are in the world std::unordered_map objects_name; - size_t object_count = _crowd_sim_interface->get_num_objects(); - for (size_t id = 0; id < object_count; id++) + for (size_t id = 0; id < _objects.size(); id++) { - auto obj = _crowd_sim_interface->get_object_by_id(id); + auto obj = _objects[id]; // already found in the Dic if (_entity_dic.find(obj->model_name) != _entity_dic.end()) continue; @@ -164,16 +363,14 @@ void CrowdSimulatorPlugin::_init_spawned_agents( { // update in entityDic _entity_dic[name->Data()] = entity; - auto obj_ptr = - _crowd_sim_interface->get_object_by_id(it_objects_name->second); + auto obj_ptr = _objects[it_objects_name->second]; // config internal spawned agent for custom trajectory if (!obj_ptr->is_external) { _config_spawned_agents(obj_ptr, entity, ecm); } objects_name.erase(name->Data()); - RCLCPP_INFO(_crowd_sim_interface->logger(), - "Crowd Simulator found agent: %s", name->Data().c_str()); + gzmsg << "Crowd Simulator found agent: " << name->Data() << std::endl; } return true; } @@ -190,17 +387,14 @@ void CrowdSimulatorPlugin::_init_spawned_agents( { // update in entityDic _entity_dic[name->Data()] = entity; - auto obj_ptr = - _crowd_sim_interface->get_object_by_id(it_objects_name->second); + auto obj_ptr = _objects[it_objects_name->second]; // config internal spawned agent for custom trajectory if (!obj_ptr->is_external) { _config_spawned_agents(obj_ptr, entity, ecm); } objects_name.erase(name->Data()); - RCLCPP_INFO(_crowd_sim_interface->logger(), - "Crowd Simulator found agent: %s", - name->Data().c_str()); + gzmsg << "Crowd Simulator found agent: " << name->Data() << std::endl; } return true; } @@ -213,27 +407,25 @@ void CrowdSimulatorPlugin::_init_spawned_agents( return; } _initialized = true; - RCLCPP_INFO( - _crowd_sim_interface->logger(), - "Ignition Models are all loaded! Start simulating..."); + gzmsg << "Gazebo Models are all loaded! Start simulating..." << std::endl; } //=================================================================== bool CrowdSimulatorPlugin::_create_entity( const std::string& model_name, - const crowd_simulator::ModelTypeDatabase::RecordPtr model_type_ptr) const + const ModelTypeDatabase::RecordPtr model_type_ptr) const { - // Use ignition create service to spawn actors - // calling ignition gazebo create service, you can use "ign service -l" to + // Use gz create service to spawn actors + // calling gz gazebo create service, you can use "ign service -l" to // check the service available assert(model_type_ptr); std::string service = "/world/" + this->_world_name + "/create"; - ignition::msgs::EntityFactory request; + gz::msgs::EntityFactory request; request.set_sdf_filename(model_type_ptr->file_name); request.set_name(model_name); - ignition::math::Pose3d pose(0, 0, 0, 0, 0, 0); + gz::math::Pose3d pose(0, 0, 0, 0, 0, 0); - ignition::msgs::Boolean response; + gz::msgs::Boolean response; bool result; uint32_t timeout = 5000; bool executed = this->_transport_node_ptr->Request(service, request, timeout, @@ -242,40 +434,34 @@ bool CrowdSimulatorPlugin::_create_entity( { if (result && response.data()) { - RCLCPP_INFO(_crowd_sim_interface->logger(), - "Requested creation of entity: %s", - model_name.c_str()); + gzmsg << "Requested creation of entity: " << model_name << std::endl; return true; } else { - RCLCPP_ERROR(_crowd_sim_interface->logger(), - "Failed request to create entity.\n %s", - request.DebugString().c_str()); + gzerr << "Failed request to create entity:" << std::endl << + request.DebugString() << std::endl; } } else { - RCLCPP_ERROR( - _crowd_sim_interface->logger(), - "Request to create entity from service %s timer out ...\n", - request.DebugString().c_str()); + gzerr << "Request to create entity from service " << + request.DebugString() << " time out ..." << std::endl; } return false; } //================================================== void CrowdSimulatorPlugin::_config_spawned_agents( - const crowd_simulator::CrowdSimInterface::ObjectPtr obj_ptr, + const ObjectPtr obj_ptr, const Entity& entity, EntityComponentManager& ecm) const { assert(obj_ptr); auto agent_ptr = obj_ptr->agent_ptr; - auto model_type = _crowd_sim_interface->_model_type_db_ptr->get( - obj_ptr->type_name); + auto model_type = _model_type_db_ptr.get(obj_ptr->type_name); // different from gazebo plugin, the pose component is the origin of the trajPose - ignition::math::Pose3d actor_pose( + gz::math::Pose3d actor_pose( static_cast(agent_ptr->_pos.x()), static_cast(agent_ptr->_pos.y()), 0.0, 0, 0, 0 @@ -291,7 +477,7 @@ void CrowdSimulatorPlugin::_config_spawned_agents( // check idle animation name auto actor_comp = ecm.Component(entity); - for (auto idle_anim : _crowd_sim_interface->get_switch_anim_name()) + for (const auto& idle_anim : _idle_animation_names) { if (actor_comp->Data().AnimationNameExists(idle_anim)) { @@ -316,16 +502,14 @@ void CrowdSimulatorPlugin::_update_all_objects( double delta_sim_time, EntityComponentManager& ecm) const { - auto objects_count = _crowd_sim_interface->get_num_objects(); - for (size_t id = 0; id < objects_count; id++) + for (size_t id = 0; id < _objects.size(); id++) { - auto obj_ptr = _crowd_sim_interface->get_object_by_id(id); + auto obj_ptr = _objects[id]; auto it_entity = _entity_dic.find(obj_ptr->model_name); if (it_entity == _entity_dic.end()) //safe check { - RCLCPP_ERROR(_crowd_sim_interface->logger(), - "Didn't initialize external agent [ %s ]", - obj_ptr->model_name.c_str()); + gzerr << "Didn't initialize external agent [ " << obj_ptr->model_name << + " ]" << std::endl; exit(EXIT_FAILURE); } auto entity = it_entity->second; @@ -335,8 +519,8 @@ void CrowdSimulatorPlugin::_update_all_objects( { auto model_pose = ecm.Component(entity)->Data(); - _crowd_sim_interface->update_external_agent(obj_ptr->agent_ptr, - model_pose); + obj_ptr->agent_ptr->_pos.setX(model_pose.Pos().X()); + obj_ptr->agent_ptr->_pos.setY(model_pose.Pos().Y()); continue; } @@ -347,18 +531,17 @@ void CrowdSimulatorPlugin::_update_all_objects( void CrowdSimulatorPlugin::_update_internal_object( double delta_sim_time, - const crowd_simulator::CrowdSimInterface::ObjectPtr obj_ptr, + const ObjectPtr obj_ptr, const Entity& entity, EntityComponentManager& ecm) const { - double animation_speed = _crowd_sim_interface->_model_type_db_ptr->get( - obj_ptr->type_name)->animation_speed; - ignition::math::Pose3d initial_pose = - _crowd_sim_interface->_model_type_db_ptr->get(obj_ptr->type_name)->pose. - convert_to_ign_math_pose_3d(); - ignition::math::Pose3d agent_pose = - _crowd_sim_interface->get_agent_pose( - obj_ptr->agent_ptr, delta_sim_time); + double animation_speed = + _model_type_db_ptr.get(obj_ptr->type_name)->animation_speed; + gz::math::Pose3d initial_pose = + _model_type_db_ptr.get(obj_ptr->type_name)->pose. + convert_to_ign_math_pose_3d(); + gz::math::Pose3d agent_pose = get_agent_pose(obj_ptr->agent_ptr, + delta_sim_time); agent_pose += initial_pose; // get components to be updated @@ -368,7 +551,7 @@ void CrowdSimulatorPlugin::_update_internal_object( ecm.Component(entity); auto anim_time_comp = ecm.Component(entity); - ignition::math::Pose3d current_pose = traj_pose_comp->Data(); + gz::math::Pose3d current_pose = traj_pose_comp->Data(); auto distance_traveled_vector = agent_pose.Pos() - current_pose.Pos(); // might need future work on 3D case // the center of human has a z_elevation, which will make the human keep walking even if he reached the target @@ -376,10 +559,9 @@ void CrowdSimulatorPlugin::_update_internal_object( double distance_traveled = distance_traveled_vector.Length(); // switch animation - auto model_type = _crowd_sim_interface->_model_type_db_ptr->get( - obj_ptr->type_name); + auto model_type = _model_type_db_ptr.get(obj_ptr->type_name); AnimState next_state = obj_ptr->get_next_state( - distance_traveled < _crowd_sim_interface->get_switch_anim_distance_th() && + distance_traveled < ANIMATION_DISTANCE_SWITCH_THRESHOLD && !model_type->idle_animation.empty()); switch (next_state) @@ -415,13 +597,152 @@ void CrowdSimulatorPlugin::_update_internal_object( ComponentState::PeriodicChange); } -IGNITION_ADD_PLUGIN( +//============================================ +ModelTypeDatabase::RecordPtr ModelTypeDatabase::emplace( + std::string type_name, + RecordPtr record_ptr) +{ + auto pair = this->_records.emplace(type_name, record_ptr); //return pair + assert(pair.second); + return pair.first->second; +} + +ModelTypeDatabase::RecordPtr ModelTypeDatabase::get( + const std::string& type_name) +const +{ + auto it = this->_records.find(type_name); + if (it == this->_records.end()) + { + std::cout << "The model type [ " << type_name << + " ] is not defined in scene file!" << std::endl; + return nullptr; + } + return it->second; +} + +size_t ModelTypeDatabase::size() const +{ + return this->_records.size(); +} + +//================================================================ +std::shared_ptr MengeHandle::init_and_make( + const std::string& resource_path, + const std::string& behavior_file, + const std::string& scene_file, + const float sim_time_step +) +{ + auto menge_handle = std::make_shared( + resource_path, behavior_file, scene_file, sim_time_step); + if (!menge_handle->_load_simulation()) + { + return nullptr; + } + return menge_handle; +} + +void MengeHandle::set_sim_time_step(float sim_time_step) +{ + this->_sim_time_step = sim_time_step; + // Set it if a valid handle is present + if (this->_sim) + { + this->_sim->setTimeStep(sim_time_step); + } +} + +float MengeHandle::get_sim_time_step() const +{ + return this->_sim_time_step; +} + +size_t MengeHandle::get_agent_count() +{ + if (this->_agent_count == 0) + { + this->_agent_count = this->_sim->getNumAgents(); + } + return this->_agent_count; +} + +void MengeHandle::sim_step() const +{ + this->_sim->step(); +} + +AgentPtr MengeHandle::get_agent(size_t id) const +{ + return AgentPtr(this->_sim->getAgent(id)); +} + +std::string MengeHandle::_resource_file_path(const std::string& relative_path) +const +{ + std::string full_path = this->_resource_path + "/" + relative_path; + std::cout << "Finding resource file: " << full_path << std::endl; + std::ifstream ifile(full_path); + if (!static_cast(ifile)) + { + std::cerr << "File not found! " << full_path << std::endl; + assert(static_cast(ifile)); + } + std::cout << "Found." << std::endl; + return full_path; +} + +bool MengeHandle::_load_simulation() +{ + Menge::SimulatorDB sim_db; + Menge::PluginEngine::CorePluginEngine engine(&sim_db); + + std::cout << "Start CrowdSimulator initializing [Menge]..." << std::endl; + + this->_sim = std::shared_ptr( + sim_db.getDBEntry("orca")->getSimulator( + this->_agent_count, + this->_sim_time_step, + 0, + std::numeric_limits::infinity(), + this->_behavior_file, + this->_scene_file, + "", + "", + false) + ); + + if (this->_sim) + { + std::cout << std::endl << "Crowd Simulator initialized success [Menge]. " << + std::endl; + return true; + } + std::cout << + "Error in provided navmesh. Menge simulator initialized false." << + std::endl; + return false; +} + +//============================================= +AnimState Object::get_next_state( + bool condition) +{ + if (condition) + return AnimState::IDLE; + else + return AnimState::WALK; + + return current_state; +} + +GZ_ADD_PLUGIN( CrowdSimulatorPlugin, System, CrowdSimulatorPlugin::ISystemConfigure, CrowdSimulatorPlugin::ISystemPreUpdate) -IGNITION_ADD_PLUGIN_ALIAS(CrowdSimulatorPlugin, +GZ_ADD_PLUGIN_ALIAS(CrowdSimulatorPlugin, "crowd_simulation") -} //namespace crowd_simulation_ign +} //namespace crowd_simulation_gz diff --git a/rmf_building_sim_gz_plugins/src/crowd_simulator.hpp b/rmf_building_sim_gz_plugins/src/crowd_simulator.hpp index 7299593..8bc97d1 100644 --- a/rmf_building_sim_gz_plugins/src/crowd_simulator.hpp +++ b/rmf_building_sim_gz_plugins/src/crowd_simulator.hpp @@ -19,71 +19,221 @@ #include -#include -#include -#include +#include +#include +#include -#include +#include -#include +#include +#include +#include +#include +#include -namespace crowd_simulation_ign { -class IGNITION_GAZEBO_VISIBLE CrowdSimulatorPlugin - : public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate +namespace crowd_simulation_gz { + +using AgentPtr = std::shared_ptr; + +enum class AnimState +{ + WALK, + IDLE, +}; + +//================================================================ +/* +* class Object +*/ +struct Object +{ + AgentPtr agent_ptr; + std::string model_name; + std::string type_name; + bool is_external = false; + AnimState current_state; + AnimState get_next_state(bool condition); +}; +using ObjectPtr = std::shared_ptr; + +class AgentPose3d +{ +public: + AgentPose3d() + : _x(0), _y(0), _z(0), _roll(0), _pitch(0), _yaw(0) + {} + AgentPose3d(double x, double y, double z, double roll, double pitch, + double yaw) + : _x(x), _y(y), _z(z), _roll(roll), _pitch(pitch), _yaw(yaw) + {} + + double x() const {return _x;} + double y() const {return _y;} + double z() const {return _z;} + double roll() const {return _roll;} + double pitch() const {return _pitch;} + double yaw() const {return _yaw;} + + void x(double x) {_x = x;} + void y(double y) {_y = y;} + void z(double z) {_z = z;} + void roll(double roll) {_roll = roll;} + void pitch(double pitch) {_pitch = pitch;} + void yaw(double yaw) {_yaw = yaw;} + + template + IgnMathPose3d convert_to_ign_math_pose_3d() + { + return IgnMathPose3d(_x, _y, _z, _roll, _pitch, _yaw); + } + +private: + double _x, _y, _z, _roll, _pitch, _yaw; +}; + +//================================================================ +/* +* class ModelTypeDatabase +*/ +class ModelTypeDatabase +{ +public: + struct Record + { + std::string type_name; + std::string file_name; + AgentPose3d pose; + std::string animation; + std::string idle_animation; + double animation_speed; + }; + + using RecordPtr = std::shared_ptr; + + //Create a new record and returns a reference to the record + RecordPtr emplace(std::string type_name, RecordPtr record_ptr); + size_t size() const; + RecordPtr get(const std::string& type_name) const; + +private: + std::unordered_map _records; +}; + +//================================================================ +/* +* class MengeHandle, provides a wrap-up class handle for actual menge lib +* only exposing several menge function interface +*/ +class MengeHandle : public std::enable_shared_from_this +{ +public: + + static std::shared_ptr init_and_make( + const std::string& resource_path, + const std::string& behavior_file, + const std::string& scene_file, + const float sim_time_step + ); + + MengeHandle(const std::string& resource_path, + const std::string& behavior_file, + const std::string& scene_file, + const float sim_time_step = 0.0 + ) + : _resource_path(resource_path), + _behavior_file(behavior_file), + _scene_file(scene_file), + _sim_time_step(sim_time_step), + _agent_count(0) + { + _behavior_file = this->_resource_file_path(_behavior_file); + _scene_file = this->_resource_file_path(_scene_file); + } + + void set_sim_time_step(float sim_time_step); + float get_sim_time_step() const; + size_t get_agent_count(); + void sim_step() const; //proceed one-time simulation step in _sim + AgentPtr get_agent(size_t id) const; + +private: + std::string _resource_path; + std::string _behavior_file; + std::string _scene_file; + float _sim_time_step; + size_t _agent_count; + std::shared_ptr _sim; + + std::string _resource_file_path(const std::string& relative_path) const; + bool _load_simulation(); //initialize simulatorinterface +}; + + +class CrowdSimulatorPlugin + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { - using AnimState = crowd_simulator::CrowdSimInterface::AnimState; public: CrowdSimulatorPlugin() - : _transport_node_ptr(std::make_shared()), - _crowd_sim_interface(std::make_shared()), + : _transport_node_ptr(std::make_shared()), _initialized(false) {} // inherit from ISystemConfigure - void Configure(const ignition::gazebo::Entity& entity, + void Configure(const gz::sim::Entity& entity, const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& event_mgr) override; + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& event_mgr) override; // inherit from ISystemPreUpdate - void PreUpdate(const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) override; + void PreUpdate(const gz::sim::UpdateInfo& info, + gz::sim::EntityComponentManager& ecm) override; private: - std::shared_ptr _transport_node_ptr; - std::shared_ptr _crowd_sim_interface; + static constexpr double ANIMATION_DISTANCE_SWITCH_THRESHOLD = 0.01; + std::shared_ptr _transport_node_ptr; bool _initialized; + bool _enabled; + std::string _resource_path; + std::string _behavior_file; + std::string _scene_file; + float _sim_time_step; + std::shared_ptr _menge_handle; + ModelTypeDatabase _model_type_db_ptr; + std::vector _external_agents; + std::vector _objects; //Database, use id to access ObjectPtr + std::vector _idle_animation_names = {"idle", "stand"}; std::chrono::steady_clock::duration _last_sim_time{0}; - std::shared_ptr _world; + std::shared_ptr _world; std::string _world_name; // map for , contains both external (models) and internal agents (actors) std::unordered_map _object_dic; // map for contains external and internal agents - std::unordered_map _entity_dic; + std::unordered_map _entity_dic; + bool read_sdf(const std::shared_ptr& sdf); bool _spawn_agents_in_world(); - void _init_spawned_agents(ignition::gazebo::EntityComponentManager& ecm); + void _init_spawned_agents(gz::sim::EntityComponentManager& ecm); void _config_spawned_agents( - const crowd_simulator::CrowdSimInterface::ObjectPtr obj_ptr, - const ignition::gazebo::Entity& enity, - ignition::gazebo::EntityComponentManager& ecm) const; + const ObjectPtr obj_ptr, + const gz::sim::Entity& enity, + gz::sim::EntityComponentManager& ecm) const; bool _create_entity( const std::string& model_name, - const crowd_simulator::ModelTypeDatabase::RecordPtr model_type_ptr) const; + const ModelTypeDatabase::RecordPtr model_type_ptr) const; void _update_all_objects( double delta_sim_time, - ignition::gazebo::EntityComponentManager& ecm) const; + gz::sim::EntityComponentManager& ecm) const; void _update_internal_object( double delta_sim_time, - const crowd_simulator::CrowdSimInterface::ObjectPtr obj_ptr, - const ignition::gazebo::Entity& enity, - ignition::gazebo::EntityComponentManager& ecm) const; + const ObjectPtr obj_ptr, + const gz::sim::Entity& enity, + gz::sim::EntityComponentManager& ecm) const; }; -} //namespace crowd_simulation_ign +} //namespace crowd_simulation_gz diff --git a/rmf_building_sim_gz_plugins/src/door.cpp b/rmf_building_sim_gz_plugins/src/door.cpp index a380d8a..ae48b75 100644 --- a/rmf_building_sim_gz_plugins/src/door.cpp +++ b/rmf_building_sim_gz_plugins/src/door.cpp @@ -1,251 +1,327 @@ -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include +#include -using namespace ignition::gazebo; +#include +#include -using namespace rmf_building_sim_common; +using namespace gz::sim; -namespace rmf_building_sim_gz_plugins { +using namespace rmf_building_sim_gz_plugins; +using DoorMode = rmf_door_msgs::msg::DoorMode; +using DoorState = rmf_door_msgs::msg::DoorState; +using DoorRequest = rmf_door_msgs::msg::DoorRequest; -//============================================================================== - -class IGNITION_GAZEBO_VISIBLE DoorPlugin +class DoorPlugin : public System, public ISystemConfigure, public ISystemPreUpdate { private: + // TODO(luca) make this a parameter of the door manager + static constexpr double PUBLISH_DT = 1.0; rclcpp::Node::SharedPtr _ros_node; - std::unordered_map _joints; + rclcpp::Publisher::SharedPtr _door_state_pub; + rclcpp::Subscription::SharedPtr _door_request_sub; - std::unordered_map _last_velocities; + // Used to do open loop joint position control + std::unordered_map _last_cmd_vel; - std::shared_ptr _door_common = nullptr; + // Saves the last timestamp a door state was sent + std::unordered_map _last_state_pub; - bool _initialized = false; bool _first_iteration = true; - void create_entity_components(Entity entity, EntityComponentManager& ecm) + Entity get_joint_entity(const EntityComponentManager& ecm, + const Entity& model_entity, + const std::string& joint_name) const { - enableComponent(ecm, entity); - } - - std::optional get_doors( - Model& model, - const std::string& door_name, - const std::shared_ptr& sdf, - EntityComponentManager& ecm) - { - // We work with a clone to avoid const correctness issues with - // get_sdf_param functions in utils.hpp - auto sdf_clone = sdf->Clone(); - std::string left_door_joint_name; - std::string right_door_joint_name; - std::string door_type; - - auto door_element = sdf_clone; - if (!get_element_required(sdf_clone, "door", door_element) || - !get_sdf_attribute_required( - door_element, "left_joint_name", left_door_joint_name) || - !get_sdf_attribute_required( - door_element, "right_joint_name", right_door_joint_name) || - !get_sdf_attribute_required( - door_element, "type", door_type)) + auto joint_entity = Model(model_entity).JointByName(ecm, joint_name); + if (joint_entity == kNullEntity) { - RCLCPP_ERROR(_ros_node->get_logger(), - " -- Missing required parameters for [%s] plugin", - door_name.c_str()); - return std::nullopt; - } - - if ((left_door_joint_name == "empty_joint" && - right_door_joint_name == "empty_joint") || - (left_door_joint_name.empty() && right_door_joint_name.empty())) - { - RCLCPP_ERROR(_ros_node->get_logger(), - " -- Both door joint names are missing for [%s] plugin, at least one" - " is required", door_name.c_str()); - return std::nullopt; + // Try for its parent (i.e. for lift nested cabin doors) + joint_entity = Model(ecm.ParentEntity(model_entity)).JointByName(ecm, + joint_name); + if (joint_entity == kNullEntity) + { + gzwarn << "Joint " << joint_name << " not found" << std::endl; + } } + return joint_entity; + } - std::unordered_set joint_names; - if (!left_door_joint_name.empty() - && left_door_joint_name != "empty_joint") - joint_names.insert(left_door_joint_name); - if (!right_door_joint_name.empty() - && right_door_joint_name != "empty_joint") - joint_names.insert(right_door_joint_name); + bool is_joint_at_position(double joint_position, double dx_min, + double target_position) const + { + return std::abs(target_position - joint_position) < dx_min; + } - DoorCommon::Doors doors; - for (auto joint_name: joint_names) + DoorModeCmp get_current_mode(const Entity& entity, + EntityComponentManager& ecm, + const DoorData& door) const + { + bool all_open = true; + bool all_closed = true; + for (const auto& joint : door.joints) { - auto joint_entity = model.JointByName(ecm, joint_name); + auto joint_entity = get_joint_entity(ecm, entity, joint.name); if (joint_entity == kNullEntity) { continue; } - const auto* joint_axis = - ecm.Component(joint_entity); - - double lower_limit = -1.57; - double upper_limit = 0.0; - if (joint_axis != nullptr) + const auto* joint_component = + ecm.Component(joint_entity); + const double joint_position = joint_component->Data()[0]; + if (!is_joint_at_position(joint_position, door.params.dx_min, + joint.open_position)) { - lower_limit = joint_axis->Data().Lower(); - upper_limit = joint_axis->Data().Upper(); + all_open = false; } - - DoorCommon::DoorElement door_element; - if (joint_name == right_door_joint_name) + if (!is_joint_at_position(joint_position, door.params.dx_min, + joint.closed_position)) { - door_element = DoorCommon::DoorElement{lower_limit, upper_limit, true}; + all_closed = false; } - else if (joint_name == left_door_joint_name) - { - door_element = DoorCommon::DoorElement{lower_limit, upper_limit}; - } - else + } + if (all_open) + return DoorModeCmp::OPEN; + else if (all_closed) + return DoorModeCmp::CLOSE; + return DoorModeCmp::MOVING; + } + + double calculate_target_velocity( + const double target, + const double current_position, + const double current_velocity, + const double dt, + const MotionParams& params) const + { + double dx = target - current_position; + if (std::abs(dx) < params.dx_min / 2.0) + dx = 0.0; + + double door_v = compute_desired_rate_of_change( + dx, current_velocity, params, dt); + + return door_v; + } + + void command_door(const Entity& entity, EntityComponentManager& ecm, + const DoorData& door, double dt, DoorModeCmp cmd) + { + auto model = Model(entity); + for (const auto& joint : door.joints) + { + auto joint_entity = get_joint_entity(ecm, entity, joint.name); + if (joint_entity != kNullEntity) { - RCLCPP_WARN( - _ros_node->get_logger(), - "Unsupported joint_name %s. Ignoring...", joint_name.c_str() - ); - continue; + auto cur_pos = + ecm.Component(joint_entity)->Data()[0]; + auto target_pos = cmd == + DoorModeCmp::OPEN ? joint.open_position : joint.closed_position; + auto target_vel = calculate_target_velocity(target_pos, cur_pos, + _last_cmd_vel[joint_entity], + dt, door.params); + ecm.CreateComponent(joint_entity, components::JointPositionReset( + {cur_pos + target_vel * dt})); + _last_cmd_vel[joint_entity] = target_vel; } - - doors.insert({joint_name, door_element}); } - return doors; } -public: - DoorPlugin() + + void publish_state(const double t, const std::string& name, + const DoorModeCmp& door_state) { + DoorState msg; + msg.door_name = name; + msg.door_time.sec = t; + msg.door_time.nanosec = (t - static_cast(t)) * 1e9; + switch (door_state) + { + case DoorModeCmp::OPEN: { + msg.current_mode.value = msg.current_mode.MODE_OPEN; + break; + } + case DoorModeCmp::MOVING: { + msg.current_mode.value = msg.current_mode.MODE_MOVING; + break; + } + case DoorModeCmp::CLOSE: { + msg.current_mode.value = msg.current_mode.MODE_CLOSED; + break; + } + } + _door_state_pub->publish(msg); } - void Configure(const Entity& entity, - const std::shared_ptr& sdf, +public: + void Configure(const Entity& /*entity*/, + const std::shared_ptr& /*sdf*/, EntityComponentManager& ecm, EventManager& /*_eventMgr*/) override { - // TODO proper rclcpp init (only once and pass args) - auto model = Model(entity); - char const** argv = NULL; - std::string name; - auto door_ele = sdf->GetElementImpl("door"); - get_sdf_attribute_required(door_ele, "name", name); if (!rclcpp::ok()) - rclcpp::init(0, argv); - std::string plugin_name("plugin_" + name); - sanitize_node_name(plugin_name); - _ros_node = std::make_shared(plugin_name); - - RCLCPP_INFO(_ros_node->get_logger(), - "Loading DoorPlugin for [%s]", - name.c_str()); - - auto doors = get_doors(model, name, sdf, ecm); + rclcpp::init(0, nullptr); - if (!doors.has_value()) - return; + _ros_node = std::make_shared("rmf_simulation_door_manager"); - _door_common = DoorCommon::make( - name, - _ros_node, - sdf, - doors.value()); + RCLCPP_INFO(_ros_node->get_logger(), + "Loading DoorManager"); - if (!_door_common) - return; + // Subscribe to door requests, publish door states + const auto pub_qos = rclcpp::QoS(100).reliable(); + _door_state_pub = _ros_node->create_publisher( + "door_states", pub_qos); - for (const auto& joint_name : _door_common->joint_names()) - { - const auto joint = model.JointByName(ecm, joint_name); - if (!joint) + _door_request_sub = _ros_node->create_subscription( + "door_requests", rclcpp::SystemDefaultsQoS(), + [ecm = &ecm](DoorRequest::UniquePtr msg) { - RCLCPP_ERROR(_ros_node->get_logger(), - " -- Model is missing the joint [%s]", - joint_name.c_str()); - return; - } - create_entity_components(joint, ecm); - _joints.insert({joint_name, joint}); - } + // Find entity with the name and create a DoorCmd component + // TODO(luca) cache this to avoid expensive iteration over all entities? + auto entity = ecm->EntityByComponents(components::Name( + msg->door_name)); + const auto* door = ecm->Component(entity); + if (entity != kNullEntity && door != nullptr) + { + if (door->Data().ros_interface == false) + { + gzmsg << "Ignoring door " << msg->door_name << " because it doesn't have a ros interface" << std::endl; + return; + } + auto door_command = msg->requested_mode.value == msg->requested_mode.MODE_OPEN ? + DoorModeCmp::OPEN : DoorModeCmp::CLOSE; + ecm->CreateComponent(entity, + components::DoorCmd(door_command)); + } + else + { + gzwarn << "Request received for door " << msg->door_name << + " but it is not being simulated" << std::endl; + } + }); + } - _initialized = true; + void initialize_components(EntityComponentManager& ecm) + { + ecm.Each([&](const Entity& entity, + const components::Door* door) -> bool + { + for (auto joint : door->Data().joints) + { + auto joint_entity = get_joint_entity(ecm, entity, joint.name); + std::vector position = {0.0}; + ecm.CreateComponent(joint_entity, + components::JointPosition(position)); + } + enableComponent(ecm, entity); + return true; + }); + } - RCLCPP_INFO(_ros_node->get_logger(), - "Finished loading [%s]", - name.c_str()); + void initialize_pub_times(EntityComponentManager& ecm) + { + ecm.Each([&](const Entity& e, + const components::Door* door_comp) -> bool + { + if (door_comp->Data().ros_interface == false) + return true; + _last_state_pub[e] = ((double) std::rand()) / + ((double) RAND_MAX/PUBLISH_DT); + return true; + }); } void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override { rclcpp::spin_some(_ros_node); - // JointPosition and JointVelocity components are populated by Physics - // system in Update, hence they are uninitialized in the first PreUpdate. - if (!_initialized || _first_iteration) + if (_first_iteration) { _first_iteration = false; + initialize_components(ecm); + initialize_pub_times(ecm); return; } - // Don't update the pose if the simulation is paused + // Don't update if the simulation is paused if (info.paused) return; - double t = - (std::chrono::duration_cast(info.simTime). - count()) * 1e-9; - double dt = - (std::chrono::duration_cast(info.dt). - count()) * 1e-9; - - // Create DoorUpdateRequest - std::vector requests; - for (const auto& joint : _joints) - { - DoorCommon::DoorUpdateRequest request; - request.joint_name = joint.first; - request.position = ecm.Component( - joint.second)->Data()[0]; - // Open loop velocity control, similar to slotcar - request.velocity = _last_velocities[request.joint_name]; - requests.push_back(request); - } - - auto results = _door_common->update(t, requests); - - // Apply motions to the joints - for (const auto& result : results) + const double t = to_seconds(info.simTime); + std::unordered_set finished_cmds; + // Process commands + ecm.Each([&](const Entity& entity, + const components::Door* door_comp, + const components::DoorCmd* door_cmd_comp, + components::DoorStateComp* door_state_comp, + const components::Name* name_comp) -> bool + { + double dt = to_seconds(info.dt); + const auto& name = name_comp->Data(); + const auto& door = door_comp->Data(); + const auto& door_cmd = door_cmd_comp->Data(); + auto& last_mode = door_state_comp->Data(); + command_door(entity, ecm, door, dt, door_cmd); + // Publish state if there was a change + const auto cur_mode = get_current_mode(entity, ecm, door); + if (cur_mode != door_state_comp->Data()) + { + last_mode = cur_mode; + if (door_comp->Data().ros_interface) + { + publish_state(t, name, cur_mode); + } + } + if (door_cmd == cur_mode) + { + finished_cmds.insert(entity); + } + return true; + }); + + // Publish states + ecm.Each([&](const Entity& e, + const components::Door* door_comp, + const components::DoorStateComp* door_state_comp, + const components::Name* name_comp) -> bool + { + if (door_comp->Data().ros_interface == false) + return true; + auto it = _last_state_pub.find(e); + if (it != _last_state_pub.end() && t - it->second >= PUBLISH_DT) + { + it->second = t; + publish_state(t, name_comp->Data(), door_state_comp->Data()); + } + return true; + }); + + for (const auto& entity: finished_cmds) { - const auto it = _joints.find(result.joint_name); - assert(it != _joints.end()); - auto cur_pos = - ecm.Component(it->second)->Data()[0]; - ecm.CreateComponent(it->second, - components::JointPositionReset({cur_pos + result.velocity * dt})); - _last_velocities[result.joint_name] = result.velocity; + enableComponent(ecm, entity, false); } } - }; -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( DoorPlugin, System, DoorPlugin::ISystemConfigure, DoorPlugin::ISystemPreUpdate) -IGNITION_ADD_PLUGIN_ALIAS(DoorPlugin, "door") - -} // namespace rmf_building_sim_gz_plugins +GZ_ADD_PLUGIN_ALIAS(DoorPlugin, "door") diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index fb1df6c..44626c5 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -1,108 +1,85 @@ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include +#include -using namespace ignition::gazebo; +#include +#include +#include -using namespace rmf_building_sim_common; +using namespace gz::sim; -namespace building_sim_ign { +using DoorMode = rmf_door_msgs::msg::DoorMode; +using LiftState = rmf_lift_msgs::msg::LiftState; +using LiftRequest = rmf_lift_msgs::msg::LiftRequest; -enum class PhysEnginePlugin {DEFAULT, TPE}; -std::unordered_map plugin_names { - {"ignition-physics-tpe-plugin", PhysEnginePlugin::TPE}}; +namespace rmf_building_sim_gz_plugins { //============================================================================== -class IGNITION_GAZEBO_VISIBLE LiftPlugin +class LiftPlugin : public System, public ISystemConfigure, public ISystemPreUpdate { private: + // TODO(luca) make this a parameter of the lift manager + static constexpr double PUBLISH_DT = 1.0; rclcpp::Node::SharedPtr _ros_node; - Entity _cabin_joint; - Entity _lift_entity; - std::vector _payloads; - ignition::math::AxisAlignedBox _initial_aabb; - ignition::math::Pose3d _initial_pose; - bool _read_aabb_dimensions = true; - PhysEnginePlugin _phys_plugin = PhysEnginePlugin::DEFAULT; - bool _first_iteration = true; + rclcpp::Publisher::SharedPtr _lift_state_pub; + rclcpp::Subscription::SharedPtr _lift_request_sub; - std::unique_ptr _lift_common = nullptr; + std::unordered_map _initial_aabbs; + std::unordered_map _initial_poses; + mutable std::unordered_map _cached_entity_by_names; - bool _initialized = false; + std::unordered_map _last_cmd_vel; + std::unordered_map _last_lift_command; + std::unordered_map _last_states; - void create_entity_components(Entity entity, EntityComponentManager& ecm) - { - enableComponent(ecm, entity); - enableComponent(ecm, entity); - const auto pos = ecm.Component(entity); - ecm.Component(entity)->Data() = pos->Data(); - } - - void create_joint_components(Entity entity, EntityComponentManager& ecm) - { - enableComponent(ecm, entity); - enableComponent(ecm, entity); - ecm.Component(entity)->Data() = {0.0}; - enableComponent(ecm, entity); - enableComponent(ecm, entity); - ecm.Component(entity)->Data() = {0.0}; - } + // Saves the last timestamp a door state was sent + std::unordered_map _last_state_pub; - void fill_physics_engine(Entity entity, EntityComponentManager& ecm) - { - Entity parent = entity; - while (ecm.ParentEntity(parent)) - { - parent = ecm.ParentEntity(parent); - } - if (ecm.EntityHasComponentType(parent, - components::PhysicsEnginePlugin().TypeId())) - { - const std::string physics_plugin_name = - ecm.Component(parent)->Data(); - const auto it = plugin_names.find(physics_plugin_name); - if (it != plugin_names.end()) - { - _phys_plugin = it->second; - } - } - } + bool _components_initialized = false; + bool _aabb_read = false; - std::vector get_payloads(EntityComponentManager& ecm) + std::vector get_payloads(EntityComponentManager& ecm, + const Entity& lift_entity) { + std::vector payloads; const auto& lift_pose = - ecm.Component(_lift_entity)->Data(); - const ignition::math::Vector3d displacement = - lift_pose.CoordPositionSub(_initial_pose); + ecm.Component(lift_entity)->Data(); + auto pose = _initial_poses.find(lift_entity); + if (pose == _initial_poses.end()) + return payloads; + auto aabb_it = _initial_aabbs.find(lift_entity); + if (aabb_it == _initial_aabbs.end()) + return payloads; + const gz::math::Vector3d displacement = + lift_pose.CoordPositionSub(pose->second); // Calculate current AABB of lift assuming it hasn't tilted/deformed - ignition::math::AxisAlignedBox lift_aabb = _initial_aabb + displacement; + gz::math::AxisAlignedBox lift_aabb = aabb_it->second + displacement; - std::vector payloads; ecm.Each( [&](const Entity& entity, const components::Model*, @@ -110,7 +87,7 @@ class IGNITION_GAZEBO_VISIBLE LiftPlugin ) -> bool { const auto payload_position = pose->Data().Pos(); - if (entity != _lift_entity) + if (entity != lift_entity) { // Could possibly check bounding box intersection too, but this suffices if (lift_aabb.Contains(payload_position)) { @@ -122,185 +99,557 @@ class IGNITION_GAZEBO_VISIBLE LiftPlugin return payloads; } -public: - LiftPlugin() + void initialize_components(EntityComponentManager& ecm) { - // TODO init ros node - // Do nothing + ecm.Each([&](const Entity& entity, + const components::Lift* lift_comp) -> bool + { + const auto& lift = lift_comp->Data(); + auto cabin_joint_entity = + Model(entity).JointByName(ecm, lift.cabin_joint); + enableComponent(ecm, entity); + enableComponent(ecm, cabin_joint_entity); + ecm.CreateComponent(cabin_joint_entity, + components::JointVelocityCmd({0.0})); + + LiftCommand lift_command; + lift_command.request_type = LiftRequest::REQUEST_AGV_MODE; + // Set the initial floor + const auto target_it = lift.floors.find(lift.initial_floor); + auto initial_floor = std::string(""); + auto target_elevation = std::numeric_limits::max(); + if (target_it != lift.floors.end()) + { + initial_floor = target_it->first; + target_elevation = target_it->second.elevation; + } + else + { + gzwarn << "Initial floor not found for lift [" << lift.name + << "], setting elevation to first floor" << std::endl; + for (const auto& [name, floor]: lift.floors) + { + if (floor.elevation < target_elevation) + { + target_elevation = floor.elevation; + initial_floor = name; + } + } + + if (lift.floors.empty()) + { + gzwarn << "The lift [" << lift.name << "] does not support any " + << "floors. This is probably an error in your building " + << "configuration." << std::endl; + target_elevation = 0.0; + } + } + lift_command.destination_floor = initial_floor; + _last_lift_command[entity] = lift_command; + + std::vector joint_position = {target_elevation}; + ecm.CreateComponent(entity, + components::JointPositionReset{joint_position}); + ecm.CreateComponent(entity, + components::LiftCmd{lift_command}); + return true; + }); } - void Configure(const Entity& entity, - const std::shared_ptr& sdf, - EntityComponentManager& ecm, EventManager& /*_eventMgr*/) override + void read_aabbs(EntityComponentManager& ecm) { - _lift_entity = entity; - //_ros_node = gazebo_ros::Node::Get(sdf); - // TODO get properties from sdf instead of hardcoded (will fail for multiple instantiations) - // TODO proper rclcpp init (only once and pass args) - auto model = Model(entity); - char const** argv = NULL; - if (!rclcpp::ok()) - rclcpp::init(0, argv); - std::string plugin_name("plugin_" + model.Name(ecm)); - sanitize_node_name(plugin_name); - _ros_node = std::make_shared(plugin_name); - - RCLCPP_INFO(_ros_node->get_logger(), - "Loading LiftPlugin for [%s]", - model.Name(ecm).c_str()); + ecm.Each([&](const Entity& entity, + const components::AxisAlignedBox* aabb, + const components::Pose* pose) -> bool + { + // Optimization: Read and store lift's pose and AABB whenever available, then + // delete the AABB component once read. Not deleting it causes rtf to drop by + // a 3-4x factor whenever the lift moves. + const double volume = aabb->Data().Volume(); + if (volume > 0 && std::isfinite(volume)) + { + // TODO(luca) this could be a component instead of a hash map + _initial_aabbs[entity] = aabb->Data(); + _initial_poses[entity] = pose->Data(); + enableComponent(ecm, entity, false); + } + return true; + }); + } - _lift_common = LiftCommon::make( - model.Name(ecm), - _ros_node, - sdf); + std::vector get_available_floors(const LiftData& lift) const + { + std::vector floors; + floors.reserve(lift.floors.size()); + for (const auto& [name, floor] : lift.floors) + { + floors.push_back(name); + } + return floors; + } - if (!_lift_common) - return; + std::string get_current_floor(const Entity& entity, + EntityComponentManager& ecm, + const LiftData& lift) const + { + // TODO update current_floor only when lift reaches its destination + const auto joint_entity = Model(entity).JointByName(ecm, lift.cabin_joint); + if (joint_entity == kNullEntity) + { + gzwarn << "Cabin entity not found" << std::endl; + return ""; + } - enableComponent(ecm, _lift_entity); + const auto lift_pos = + ecm.Component(joint_entity)->Data()[0]; - _cabin_joint = model.JointByName(ecm, _lift_common->get_joint_name()); - if (!_cabin_joint) + double smallest_error = std::numeric_limits::infinity(); + std::string closest_floor_name; + for (const auto& [name, floor] : lift.floors) { - RCLCPP_ERROR(_ros_node->get_logger(), - " -- Model is missing the joint [%s]", - _lift_common->get_joint_name().c_str()); - return; + double tmp_error = abs(lift_pos - floor.elevation); + if (tmp_error < smallest_error) + { + smallest_error = tmp_error; + closest_floor_name = name; + } } + return closest_floor_name; + } - _initialized = true; + // Returns all the doors for the given floor + std::vector get_floor_doors(EntityComponentManager& ecm, + const LiftData& lift, + const std::string& floor_name) const + { + std::vector doors; + const auto floor_it = lift.floors.find(floor_name); + if (floor_it == lift.floors.end()) + { + return doors; + } - RCLCPP_INFO(_ros_node->get_logger(), - "Finished loading [%s]", - model.Name(ecm).c_str()); + for (const auto& door_pair : floor_it->second.doors) + { + const auto shaft_door = entity_by_name(ecm, door_pair.shaft_door); + if (shaft_door != kNullEntity) + doors.push_back(shaft_door); + const auto cabin_door = entity_by_name(ecm, door_pair.cabin_door); + if (cabin_door != kNullEntity) + doors.push_back(cabin_door); + } + return doors; } - void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override + uint8_t get_door_state(const Entity& entity, EntityComponentManager& ecm, + const LiftData& lift) const { - // Read from components that may not have been initialized in configure() - if (_first_iteration) - { - fill_physics_engine(_lift_entity, ecm); + auto cur_floor = get_current_floor(entity, ecm, lift); + auto doors = get_floor_doors(ecm, lift, cur_floor); - double lift_elevation = _lift_common->get_elevation(); - if (_phys_plugin == PhysEnginePlugin::DEFAULT) - { - create_joint_components(_cabin_joint, ecm); - auto position_cmd = ecm.Component( - _cabin_joint); - position_cmd->Data()[0] = lift_elevation; - } - else + bool all_open = true; + bool all_closed = true; + + for (const auto& door: doors) + { + const auto door_state = ecm.Component(door); + if (door_state == nullptr) { - create_entity_components(_lift_entity, ecm); - auto position_cmd = ecm.Component( - _lift_entity); - position_cmd->Data().Pos().Z() = lift_elevation; + gzwarn << "Door state for lift not found" << std::endl; + continue; } + if (door_state->Data() != DoorModeCmp::OPEN) + all_open = false; + if (door_state->Data() != DoorModeCmp::CLOSE) + all_closed = false; + } - _first_iteration = false; - return; + if (all_open) + return DoorMode::MODE_OPEN; + else if (all_closed) + return DoorMode::MODE_CLOSED; + return DoorMode::MODE_MOVING; + } + + uint8_t get_motion_state(const Entity& entity, EntityComponentManager& ecm, + const LiftData& lift, + const std::string& destination_floor) const + { + const auto joint_entity = Model(entity).JointByName(ecm, lift.cabin_joint); + if (joint_entity == kNullEntity) + { + gzwarn << "Cabin entity not found" << std::endl; + return LiftState::MOTION_STOPPED; } - // Optimization: Read and store lift's pose and AABB whenever available, then - // delete the AABB component once read. Not deleting it causes rtf to drop by - // a 3-4x factor whenever the lift moves. - if (_read_aabb_dimensions) + const auto lift_pos = + ecm.Component(joint_entity)->Data()[0]; + const auto target_it = lift.floors.find(destination_floor); + + if (target_it != lift.floors.end()) { - const auto& aabb_component = - ecm.Component(_lift_entity); - const auto& pose_component = - ecm.Component(_lift_entity); + const auto& target_elevation = target_it->second.elevation; + if (std::abs(target_elevation - lift_pos) < lift.params.dx_min) + return LiftState::MOTION_STOPPED; + if (target_elevation - lift_pos > 0) + return LiftState::MOTION_UP; + if (target_elevation - lift_pos < 0) + return LiftState::MOTION_DOWN; + } + return LiftState::MOTION_STOPPED; + } + + void command_doors(EntityComponentManager& ecm, + const std::vector& doors, + DoorModeCmp door_state) const + { + for (const auto& entity : doors) + ecm.CreateComponent(entity, + components::DoorCmd(door_state)); + } - if (aabb_component && pose_component) + // TODO(luca) move this to a common block and reuse in all plugins + Entity entity_by_name(EntityComponentManager& ecm, + const std::string& name) const + { + // Lookup the cache first + auto it = _cached_entity_by_names.find(name); + if (it != _cached_entity_by_names.end()) + return it->second; + auto entity = ecm.EntityByComponents(components::Name(name)); + if (entity != kNullEntity) + { + _cached_entity_by_names[name] = entity; + } + return entity; + } + + bool all_doors_at_state(EntityComponentManager& ecm, + const std::vector& doors, + DoorModeCmp cmd) const + { + for (const auto& entity : doors) + { + const auto* door_state_comp = ecm.Component( + entity); + // TODO log + if (door_state_comp == nullptr) { - const double volume = aabb_component->Data().Volume(); - if (volume > 0 && volume != std::numeric_limits::infinity()) - { - _initial_aabb = aabb_component->Data(); - _initial_pose = pose_component->Data(); - enableComponent(ecm, _lift_entity, false); - _read_aabb_dimensions = false; - } + continue; } + if (door_state_comp->Data() != cmd) + return false; } + return true; + } - // TODO parallel thread executor? - rclcpp::spin_some(_ros_node); - if (!_initialized) - return; - - // Don't update the pose if the simulation is paused - if (info.paused) - return; + double calculate_target_velocity( + const double target, + const double current_position, + const double current_velocity, + const double dt, + const MotionParams& params) const + { + double dx = target - current_position; + if (std::abs(dx) < params.dx_min / 2.0) + dx = 0.0; - // Send update request - const double t = - (std::chrono::duration_cast(info.simTime). - count()) * 1e-9; - double position = 0.0; - double velocity = 0.0; + return compute_desired_rate_of_change( + dx, current_velocity, params, dt); + } - // Read from either joint data or model data based on physics engine - if (_phys_plugin == PhysEnginePlugin::DEFAULT) + double command_lift(const Entity& entity, EntityComponentManager& ecm, + const LiftData& lift, double dt, double target_elevation, + double cur_elevation) + { + auto joint_entity = Model(entity).JointByName(ecm, lift.cabin_joint); + auto target_vel = 0.0; + if (joint_entity != kNullEntity) { - position = ecm.Component( - _cabin_joint)->Data()[0]; - velocity = ecm.Component( - _cabin_joint)->Data()[0]; + target_vel = calculate_target_velocity(target_elevation, cur_elevation, + _last_cmd_vel[joint_entity], dt, + lift.params); + ecm.Component(joint_entity)->Data() = + {target_vel}; + _last_cmd_vel[joint_entity] = target_vel; } - else + return target_vel; + } + + LiftState get_current_state(const Entity& entity, EntityComponentManager& ecm, + const LiftData& lift, + const components::LiftCmd* lift_cmd) + { + LiftState msg; + + msg.available_floors = get_available_floors(lift); + msg.current_floor = get_current_floor(entity, ecm, lift); + msg.destination_floor = lift_cmd != nullptr ? + lift_cmd->Data().destination_floor : msg.current_floor; + + msg.door_state = get_door_state(entity, ecm, lift); + msg.motion_state = lift_cmd != nullptr ? + get_motion_state(entity, ecm, lift, + lift_cmd->Data().destination_floor) : msg.MOTION_STOPPED; + msg.current_mode = msg.MODE_AGV; + msg.session_id = lift_cmd != nullptr ? + lift_cmd->Data().session_id : ""; + auto it = _last_lift_command.find(entity); + if (it != _last_lift_command.end()) { - position = ecm.Component( - _lift_entity)->Data().Pos().Z(); - auto lift_vel_cmd = ecm.Component( - _lift_entity); - velocity = lift_vel_cmd->Data()[2]; + msg.session_id = it->second.session_id; } + return msg; + } - auto result = _lift_common->update(t, position, velocity); +public: - // Move either joint or lift cabin based on physics engine used - if (_phys_plugin == PhysEnginePlugin::DEFAULT) + void Configure(const Entity& /*entity*/, + const std::shared_ptr& /*sdf*/, + EntityComponentManager& ecm, EventManager& /*_eventMgr*/) override + { + if (!rclcpp::ok()) + rclcpp::init(0, nullptr); + std::string plugin_name("rmf_simulation_lift_manager"); + _ros_node = std::make_shared(plugin_name); + + const auto reliable_qos = rclcpp::QoS(100).reliable(); + _lift_state_pub = _ros_node->create_publisher( + "lift_states", reliable_qos); + + _lift_request_sub = _ros_node->create_subscription( + "lift_requests", reliable_qos, + [this, ecm = &ecm](LiftRequest::UniquePtr msg) + { + // Find entity with the name and create a DoorCmd component + auto entity = entity_by_name(*ecm, msg->lift_name); + const auto* lift_comp = ecm->Component(entity); + if (entity != kNullEntity && lift_comp != nullptr) + { + const auto& available_floors = lift_comp->Data().floors; + if (available_floors.find(msg->destination_floor) == + available_floors.end()) + { + gzwarn << "Received request for unavailable floor [" << msg->destination_floor << "]" << std::endl; + return; + } + LiftCommand lift_command; + lift_command.request_type = msg->request_type; + lift_command.destination_floor = msg->destination_floor; + lift_command.session_id = msg->request_type == msg->REQUEST_END_SESSION ? + "" : msg->session_id; + + lift_command.door_state = msg->door_state == msg->DOOR_OPEN ? + DoorModeCmp::OPEN : DoorModeCmp::CLOSE; + + // Trigger an error if a request, different from previous one, comes in + const auto* cur_lift_cmd_comp = + ecm->Component(entity); + if (cur_lift_cmd_comp) + { + const auto& cur_lift_cmd = cur_lift_cmd_comp->Data(); + if (cur_lift_cmd.destination_floor != msg->destination_floor || + cur_lift_cmd.request_type != msg->request_type || + cur_lift_cmd.session_id != msg->session_id) + { + gzwarn << "Discarding request: [" << msg->lift_name <<"] is busy at the moment" << std::endl; + return; + } + } + else + { + auto it = _last_lift_command.find(entity); + if (it != _last_lift_command.end() && + (it->second.destination_floor != msg->destination_floor || + it->second.request_type != msg->request_type || + it->second.session_id != msg->session_id)) + { + RCLCPP_INFO(_ros_node->get_logger(), + "Lift [%s] requested at level [%s]", + msg->lift_name.c_str(), msg->destination_floor.c_str()); + _last_lift_command[entity] = lift_command; + ecm->CreateComponent(entity, + components::LiftCmd(lift_command)); + } + } + } + else + { + gzwarn << "Request received for lift " << msg->lift_name << + " but it is not being simulated" << std::endl; + } + }); + + RCLCPP_INFO(_ros_node->get_logger(), + "Starting LiftManager"); + } + + void initialize_pub_times(EntityComponentManager& ecm) + { + ecm.Each([&](const Entity& e, + const components::Lift*) -> bool + { + _last_state_pub[e] = ((double) std::rand()) / + ((double) RAND_MAX/PUBLISH_DT); + return true; + }); + } + + void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override + { + // Read from components that may not have been initialized in configure() + if (!_components_initialized) { - auto vel_cmd = ecm.Component( - _cabin_joint); - vel_cmd->Data()[0] = result.velocity; + initialize_components(ecm); + initialize_pub_times(ecm); + _components_initialized = true; + return; } - else + + if (!_aabb_read) { - auto lift_vel_cmd = ecm.Component( - _lift_entity); - lift_vel_cmd->Data()[2] = result.velocity; + read_aabbs(ecm); + _aabb_read = true; } - // Move any payloads that need to be manually moved - // (i.e. have a LinearVelocityCmd component that exists) - if (_lift_common->motion_state_changed()) + rclcpp::spin_some(_ros_node); + + // Don't update the pose if the simulation is paused + if (info.paused) + return; + + std::unordered_set finished_cmds; + + const double dt = to_seconds(info.dt); + // Command lifts + ecm.Each([&](const Entity& entity, + const components::Lift* lift_comp, + const components::Pose* pose_comp, + const components::LiftCmd* lift_cmd_comp) -> bool + { + const auto& lift = lift_comp->Data(); + const auto& pose = pose_comp->Data(); + + const auto& lift_cmd = lift_cmd_comp->Data(); + + const auto& destination_floor = lift_cmd.destination_floor; + const double target_elevation = lift.floors.at( + destination_floor).elevation; + const auto target_door_state = lift_cmd.door_state; + const std::string cur_floor = get_current_floor(entity, ecm, lift); + + const auto doors = get_floor_doors(ecm, lift, cur_floor); + + if (std::abs(pose.Z() - target_elevation) < lift.params.dx_min) + { + // Just command the doors to the target state + command_doors(ecm, doors, target_door_state); + // Clear the command if it was finished + if (destination_floor == cur_floor && + all_doors_at_state(ecm, doors, target_door_state)) + finished_cmds.insert(entity); + } + else + { + // Make sure doors are closed before moving to next floor + command_doors(ecm, doors, DoorModeCmp::CLOSE); + if (all_doors_at_state(ecm, doors, DoorModeCmp::CLOSE)) + { + auto target_velocity = + command_lift(entity, ecm, lift, dt, target_elevation, pose.Z()); + // Move payloads as well + if (target_velocity != 0.0) + { + auto payloads = get_payloads(ecm, entity); + for (const Entity& payload : payloads) + { + if (ecm.EntityHasComponentType(payload, + components::LinearVelocityCmd().TypeId())) + { + auto lin_vel_cmd = + ecm.Component(payload); + lin_vel_cmd->Data()[2] = target_velocity; + } + } + } + } + } + + return true; + }); + + const double t = to_seconds(info.simTime); + // Update states + ecm.Each([&](const Entity& entity, + const components::Lift* lift_comp, + const components::Name* name_comp) -> bool + { + const auto& name = name_comp->Data(); + const auto& lift = lift_comp->Data(); + + const auto* lift_cmd_comp = ecm.Component(entity); + + auto current_state = get_current_state(entity, ecm, lift, + lift_cmd_comp); + // Will compare to default initialized last state which should be true at startup + if (current_state != _last_states[entity]) + { + _last_states[entity] = current_state; + current_state.lift_name = name; + current_state.lift_time.sec = t; + current_state.lift_time.nanosec = (t - static_cast(t)) * 1e9; + + _lift_state_pub->publish(current_state); + } + + return true; + }); + + // Clear finished commands + for (const auto& e : finished_cmds) { - _payloads = get_payloads(ecm); + enableComponent(ecm, e, false); } - for (const Entity& payload : _payloads) - { - if (ecm.EntityHasComponentType(payload, - components::LinearVelocityCmd().TypeId())) + // Publish state + ecm.Each([&](const Entity& e, + const components::Lift* lift_comp, + const components::Name* name_comp) -> bool { - auto lin_vel_cmd = - ecm.Component(payload); - lin_vel_cmd->Data()[2] = result.velocity; - } - } + auto it = _last_state_pub.find(e); + if (it != _last_state_pub.end() && t - it->second >= PUBLISH_DT) + { + it->second = t; + const auto& name = name_comp->Data(); + const auto& lift = lift_comp->Data(); + + const auto* lift_cmd_comp = ecm.Component( + e); + + auto msg = get_current_state(e, ecm, lift, lift_cmd_comp); + _last_states[e] = msg; + msg.lift_time.sec = t; + msg.lift_time.nanosec = (t - static_cast(t)) * 1e9; + msg.lift_name = name; + _lift_state_pub->publish(msg); + } + return true; + }); } }; -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( LiftPlugin, System, LiftPlugin::ISystemConfigure, LiftPlugin::ISystemPreUpdate) -IGNITION_ADD_PLUGIN_ALIAS(LiftPlugin, "lift") +GZ_ADD_PLUGIN_ALIAS(LiftPlugin, "lift") -} // namespace building_sim_ign +} // namespace building_sim_gz_plugins diff --git a/rmf_building_sim_gz_plugins/src/register_component.cpp b/rmf_building_sim_gz_plugins/src/register_component.cpp new file mode 100644 index 0000000..75fecdd --- /dev/null +++ b/rmf_building_sim_gz_plugins/src/register_component.cpp @@ -0,0 +1,241 @@ +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +using namespace gz::sim; + +namespace rmf_building_sim_gz_plugins { + +//============================================================================== + +class RegisterComponentPlugin + : public System, + public ISystemConfigure +{ +private: + std::optional read_door_data( + const Entity& entity, + EntityComponentManager& ecm, + const std::string& door_name, + const std::shared_ptr& sdf) + { + DoorData data; + std::string right_joint_name; + std::string left_joint_name; + + auto door_element = sdf->FindElement("door"); + + if (door_element == nullptr || + !door_element->Get("left_joint_name", left_joint_name, "") || + !door_element->Get("right_joint_name", right_joint_name, "")) + { + gzerr << "Missing required parameters for plugin " << door_name << + std::endl; + return std::nullopt; + } + + if ((left_joint_name == "empty_joint" && + right_joint_name == "empty_joint") || + (left_joint_name.empty() && right_joint_name.empty())) + { + gzerr << "Both door joint names are missing for " << door_name << + " plugin, at least one" + " is required" << std::endl; + return std::nullopt; + } + + std::unordered_set joint_names; + if (!left_joint_name.empty() + && left_joint_name != "empty_joint") + joint_names.insert(left_joint_name); + if (!right_joint_name.empty() + && right_joint_name != "empty_joint") + joint_names.insert(right_joint_name); + + sdf->Get("v_max_door", data.params.v_max, 0.2); + sdf->Get("a_max_door", data.params.a_max, 0.2); + sdf->Get("a_nom_door", data.params.a_nom, 0.08); + sdf->Get("dx_min_door", data.params.dx_min, 0.01); + sdf->Get("f_max_door", data.params.f_max, 100.0); + sdf->Get("ros_interface", data.ros_interface, false); + + auto model = Model(entity); + for (auto joint_name: joint_names) + { + auto joint_entity = model.JointByName(ecm, joint_name); + if (joint_entity == kNullEntity) + { + // Try in the global space + joint_entity = ecm.EntityByComponents(components::Name(joint_name)); + if (joint_entity == kNullEntity) + { + // TODO(luca) error handling here + std::cout << "Joint " << joint_name << " not found" << std::endl; + continue; + } + } + const auto* joint_axis = + ecm.Component(joint_entity); + + double lower_limit = -1.57; + double upper_limit = 0.0; + if (joint_axis != nullptr) + { + lower_limit = joint_axis->Data().Lower(); + upper_limit = joint_axis->Data().Upper(); + } + + if (joint_name == right_joint_name) + { + data.joints.push_back({joint_name, lower_limit, upper_limit}); + } + else if (joint_name == left_joint_name) + { + // Left joint is flipped + data.joints.push_back({joint_name, upper_limit, lower_limit}); + } + else + { + gzwarn << "Unsupported joint_name " << joint_name << " Ignoring..." << + std::endl; + continue; + } + + } + return data; + } + + std::optional read_lift_data( + const std::string& lift_name, + const std::shared_ptr& sdf) + { + LiftData data; + + // load lift cabin motion parameters + sdf->Get("v_max_cabin", data.params.v_max, 0.2); + sdf->Get("a_max_cabin", data.params.a_max, 0.2); + sdf->Get("a_nom_cabin", data.params.a_nom, 0.08); + sdf->Get("dx_min_cabin", data.params.dx_min, 0.01); + sdf->Get("f_max_cabin", data.params.f_max, 100.0); + + if (!sdf->Get("cabin_joint_name", data.cabin_joint, "")) + return std::nullopt; + + // load the floor name and elevation for each floor + auto floor_element = sdf->FindElement("floor"); + if (!floor_element) + { + gzerr << "Missing required floor element for [" << lift_name << + "] plugin" << std::endl; + return std::nullopt; + } + + std::optional first_found_floor; + while (floor_element) + { + Floor floor; + std::string floor_name; + if (!floor_element->Get("name", floor_name, "") || + !floor_element->Get("elevation", floor.elevation, 0.0)) + { + gzerr << "Missing required floor name or elevation attributes for [" << + lift_name << "] plugin" << std::endl; + return std::nullopt; + } + + if (!first_found_floor.has_value()) + first_found_floor = floor_name; + + auto door_pair_element = floor_element->FindElement("door_pair"); + while (door_pair_element) + { + FloorDoorPair doors; + if (!door_pair_element->Get("cabin_door", doors.cabin_door, + "") || + !door_pair_element->Get("shaft_door", doors.shaft_door, + "")) + { + gzerr << "Missing required lift door attributes for [" << + lift_name << "] plugin" << std::endl; + return std::nullopt; + } + floor.doors.push_back(doors); + + door_pair_element = door_pair_element->GetNextElement("door_pair"); + } + + data.floors.insert({floor_name, floor}); + floor_element = floor_element->GetNextElement("floor"); + } + + if (!first_found_floor.has_value()) + { + gzerr << "No floors enabled for [" << lift_name << "] plugin" << + std::endl; + return std::nullopt; + } + + sdf->Get("initial_floor", data.initial_floor, ""); + + if (data.floors.find(data.initial_floor) == data.floors.end()) + { + gzerr << "Initial floor [" << data.initial_floor << + "] not available, changing to default" << std::endl; + data.initial_floor = *first_found_floor; + } + + return data; + } + + +public: + void Configure(const Entity& entity, + const std::shared_ptr& sdf, + EntityComponentManager& ecm, EventManager& /*_eventMgr*/) override + { + if (sdf->HasElement("component")) + { + auto component_element = sdf->FindElement("component"); + while (component_element) + { + auto name = component_element->Get("name"); + if (name == "Door") + { + if (auto data = read_door_data(entity, ecm, name, component_element)) + ecm.CreateComponent(entity, components::Door(*data)); + } + else if (name == "Lift") + { + if (auto data = read_lift_data(name, component_element)) + { + ecm.CreateComponent(entity, components::Lift(*data)); + // Bounding boxes are needed to move payloads, enable here to + // simplify logic at the lift plugin level + enableComponent(ecm, entity); + } + } + component_element = component_element->GetNextElement("component"); + } + } + } +}; + +GZ_ADD_PLUGIN( + RegisterComponentPlugin, + System, + RegisterComponentPlugin::ISystemConfigure) + +GZ_ADD_PLUGIN_ALIAS(RegisterComponentPlugin, "register_component") + +} // namespace rmf_building_sim_gz_plugins diff --git a/rmf_building_sim_gz_plugins/src/toggle_charging/toggle_charging.cpp b/rmf_building_sim_gz_plugins/src/toggle_charging/toggle_charging.cpp index aace3ac..075b321 100644 --- a/rmf_building_sim_gz_plugins/src/toggle_charging/toggle_charging.cpp +++ b/rmf_building_sim_gz_plugins/src/toggle_charging/toggle_charging.cpp @@ -18,14 +18,14 @@ #include #include -#include -#include -#include +#include +#include +#include -#include -#include +#include +#include -using namespace ignition; +using namespace gz; using namespace gui; class toggle_charging : public Plugin @@ -40,8 +40,8 @@ class toggle_charging : public Plugin const std::string _enable_instant_charge_str = "_enable_instant_charge"; const std::string _enable_drain_str = "_enable_drain"; - ignition::transport::Node _node; - ignition::transport::Node::Publisher _charge_state_pub; + gz::transport::Node _node; + gz::transport::Node::Publisher _charge_state_pub; void checkbox_checked(const std::string& name, bool checked); @@ -59,7 +59,7 @@ protected slots: toggle_charging::toggle_charging() { - _charge_state_pub = _node.Advertise( + _charge_state_pub = _node.Advertise( "/charge_state"); if (!_charge_state_pub) { @@ -97,7 +97,7 @@ void toggle_charging::OnEnableDrain(bool checked) void toggle_charging::checkbox_checked( const std::string& name, bool checked) { - ignition::msgs::Selection selection; + gz::msgs::Selection selection; selection.set_name(name); selection.set_selected(checked); selection.set_id(1); // Id not necessary for current use case @@ -105,8 +105,8 @@ void toggle_charging::checkbox_checked( } // Register this plugin -IGNITION_ADD_PLUGIN(toggle_charging, - ignition::gui::Plugin) +GZ_ADD_PLUGIN(toggle_charging, + gz::gui::Plugin) #include "toggle_charging.moc" \ No newline at end of file diff --git a/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp b/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp index 2ea5a96..cda9078 100644 --- a/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp +++ b/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp @@ -18,18 +18,20 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include +#include -#include -#include -#include -#include +#include + +#include +#include +#include +#include #include #include @@ -40,7 +42,7 @@ using RobotState = rmf_fleet_msgs::msg::RobotState; class toggle_floors : - public ignition::gui::Plugin + public gz::gui::Plugin { Q_OBJECT @@ -58,7 +60,7 @@ class toggle_floors bool eventFilter(QObject* _obj, QEvent* _event) override; void PerformRenderingOperations(); void FindScene(); - ignition::rendering::ScenePtr scene{nullptr}; + gz::rendering::ScenePtr scene{nullptr}; void get_plugin_config(); QStringList qfloors; @@ -87,20 +89,20 @@ toggle_floors::toggle_floors() toggle_floors::~toggle_floors() { - ignition::gui::App()->findChild< - ignition::gui::MainWindow*>()->removeEventFilter(this); + gz::gui::App()->findChild< + gz::gui::MainWindow*>()->removeEventFilter(this); } -// Get plugin information from ignition transport +// Get plugin information from gz transport void toggle_floors::get_plugin_config() { - ignition::transport::Node node; + gz::transport::Node node; bool result{false}; unsigned int timeout{2000}; const std::string WORLD_NAME = "sim_world"; - ignition::msgs::GUI res; - std::string service = ignition::transport::TopicUtils::AsValidTopic("/world/" + WORLD_NAME + + gz::msgs::GUI res; + std::string service = gz::transport::TopicUtils::AsValidTopic("/world/" + WORLD_NAME + "/gui/info"); node.Request(service, timeout, res, result); @@ -179,8 +181,8 @@ void toggle_floors::LoadConfig(const tinyxml2::XMLElement* _pluginElem) }); this->Context()->setContextProperty("qfloors", qfloors); - ignition::gui::App()->findChild< - ignition::gui::MainWindow*>()->installEventFilter(this); + gz::gui::App()->findChild< + gz::gui::MainWindow*>()->installEventFilter(this); } void toggle_floors::OnFloorChecked(const QString floor, bool checked) @@ -214,11 +216,11 @@ void toggle_floors::PerformRenderingOperations() auto target_node = this->scene->NodeByName(model); if (target_node == NULL) { - ignwarn << "Node for " << model << "was not found" <(target_node); + std::dynamic_pointer_cast(target_node); target_vis->SetVisible(_floor_visibility[floor_name]); } flags_itr.second = false; @@ -230,10 +232,10 @@ void toggle_floors::PerformRenderingOperations() auto target_node = this->scene->NodeByName(robots_itr.first); if (target_node == NULL) { - ignwarn << "Node for " << robots_itr.first << "was not found" <( + auto target_vis = std::dynamic_pointer_cast( target_node); target_vis->SetVisible(_floor_visibility[robots_itr.second]); } @@ -241,9 +243,9 @@ void toggle_floors::PerformRenderingOperations() void toggle_floors::FindScene() { - auto loadedEngNames = ignition::rendering::loadedEngines(); + auto loadedEngNames = gz::rendering::loadedEngines(); auto engineName = loadedEngNames[0]; - auto engine = ignition::rendering::engine(engineName); + auto engine = gz::rendering::engine(engineName); auto scenePtr = engine->SceneByIndex(0); if (!scenePtr->IsInitialized() || nullptr == scenePtr->RootVisual()) @@ -255,7 +257,7 @@ void toggle_floors::FindScene() bool toggle_floors::eventFilter(QObject* _obj, QEvent* _event) { - if (_event->type() == ignition::gui::events::Render::kType) + if (_event->type() == gz::gui::events::Render::kType) { this->PerformRenderingOperations(); } @@ -263,9 +265,9 @@ bool toggle_floors::eventFilter(QObject* _obj, QEvent* _event) } // Register this plugin -IGNITION_ADD_PLUGIN(toggle_floors, - ignition::gui::Plugin +GZ_ADD_PLUGIN(toggle_floors, + gz::gui::Plugin ) -#include "toggle_floors.moc" \ No newline at end of file +#include "toggle_floors.moc" diff --git a/rmf_robot_sim_gz_classic_plugins/CHANGELOG.rst b/rmf_robot_sim_gz_classic_plugins/CHANGELOG.rst deleted file mode 100644 index 3e0130f..0000000 --- a/rmf_robot_sim_gz_classic_plugins/CHANGELOG.rst +++ /dev/null @@ -1,58 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rmf\_robot\_sim\_gz\_classic\_plugins -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.2.2 (2023-12-20) ------------------- -* Sanitize node names to avoid plugin exceptions (`#110 `_) -* Contributors: Luca Della Vedova - -2.2.1 (2023-06-30) ------------------- - -2.2.0 (2023-06-08) ------------------- - -2.1.0 (2023-06-06) ------------------- -* Switch to rst changelogs (`#101 `_) -* Contributors: Yadunund - -2.0.0 (2022-10-04) ------------------- -* Renamed to rmf\_robot\_sim\_gz\_classic\_plugins and Humble migration (`#77 `_) -* Checking for models with dispensable in names to not be considered an obstacle by slotcar (`#74 `_) -* Contributors: Aaron Chong, Luca Della Vedova, Yadunund - -1.3.1 (2021-30-11) ------------------- -* Making install location package specific to fix (`#100 `_). (`#60 `_) -* Contributors: Marco A. Gutierrez - -1.3.0 (2020-09-01) ------------------- -* Change parameter tag from holonomic to steering (`#46 `_) -* Nonholonomic slotcar turning fixes, add turning multiplier offset parameter, rename compute\_ds variables (`#41 `_) -* fix typo on gazebo\_dev dependency (`#28 `_) -* Fix dependencies (`#26 `_) -* Changes and corrections to support ROS 2 Galactic (`#23 `_) -* Add quality declaration documents (`#1 `_) -* Add build and style actions (`#11 `_) -* Slotcar plugin package move and utils cleanup (`#5 `_) -* account for renaming from building\_map\_msgs to rmf\_building\_map\_msgs (`#3 `_) -* Package renames (`#2 `_) -* Contributors: Charayaphan Nakorn Boon Han, Geoffrey Biggs, Luca Della Vedova, Marco A. Gutiérrez, ddengster - -1.1.0 (2020-09-23) ------------------- -* Support cross-compiling with ROS 2 Foxy and Eloquent. (`#103 `_) -* TeleportIngestorPlugin uses rmf\_ingestor\_msgs. (`#117 `_) -* Modularized Readonly TeleportIngestorPlugin and TeleportDispenserPlugin to support both Gazebo and Ignition simulations. (`#124, #134 `_) -* Contributors: Aaron Chong, Boon Han, Michael X. Grey, Yadunund, mrushyendra - -1.0.0 (2020-06-24) ------------------- -* Provides the `readonly` Gazebo plugin that emulates the behavior of a `read\_only` fleet type. When attached to a steerable vehicle that operates on a known traffic graph, the plugin publishes a prediction of the vehicle\'s intended path to the `rmf traffic database` via its configured `read_only_fleet_adapter`. Other fleets operating in the space can plan routes to avoid the path of this vehicle. -* Provides `TeleportDispenser` Gazebo plugin that is attached to the `TeleportDispenser` model in `rmf_demo_assets`. When loaded into Gazebo along side a \"payload\" model, the plugin will teleport the payload onto the nearest robot when it registers a `rmf_dispenser_msgs::DispenserRequest` message with `target_guid` matching its model name. -* Provides `TeleportIngestor` Gazebo plugin that is attached to the `TeleportIngestor` model in `rmf_demo_assets`. When loaded into Gazebo, the plugin will teleport the payload off the nearest robot and onto its location, when it registers a `rmf_dispenser_msgs::DispenserRequest` message with `target_guid` matching its model name. -* Contributors: Aaron, Aaron Chong, Charayaphan Nakorn Boon Han, Luca Della Vedova, Yadunund diff --git a/rmf_robot_sim_gz_classic_plugins/CMakeLists.txt b/rmf_robot_sim_gz_classic_plugins/CMakeLists.txt deleted file mode 100644 index bd34e21..0000000 --- a/rmf_robot_sim_gz_classic_plugins/CMakeLists.txt +++ /dev/null @@ -1,137 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rmf_robot_sim_gz_classic_plugins) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(gazebo_ros REQUIRED) -find_package(gazebo_dev REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rmf_fleet_msgs REQUIRED) -find_package(rmf_building_map_msgs REQUIRED) -find_package(rmf_robot_sim_common REQUIRED) - -include(GNUInstallDirs) - -############################### -# TeleportDispenser Plugin -############################### - -add_library(teleport_dispenser SHARED src/TeleportDispenser.cpp) - -ament_target_dependencies(teleport_dispenser - rclcpp - gazebo_ros - rmf_fleet_msgs - rmf_robot_sim_common - Eigen3 -) - -target_include_directories(teleport_dispenser - PUBLIC - $ - $ - ${GAZEBO_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -############################### -# TeleportIngestor Plugin -############################### - -add_library(teleport_ingestor SHARED src/TeleportIngestor.cpp) - -ament_target_dependencies(teleport_ingestor - rclcpp - gazebo_ros - rmf_fleet_msgs - rmf_robot_sim_common - Eigen3 -) - -target_include_directories(teleport_ingestor - PUBLIC - $ - $ - ${GAZEBO_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -############################### -# Readonly Plugin -############################### - -add_library(readonly SHARED ${PROJECT_SOURCE_DIR}/src/readonly.cpp) - -ament_target_dependencies(readonly - rmf_fleet_msgs - rmf_building_map_msgs - rclcpp - gazebo_ros - rmf_robot_sim_common - Eigen3 -) - -target_include_directories(readonly - PUBLIC - $ - $ - ${GAZEBO_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -############################### -# Slotcar Plugin -############################### - -add_library(slotcar SHARED ${PROJECT_SOURCE_DIR}/src/slotcar.cpp) - -ament_target_dependencies(slotcar - Eigen3 - rmf_robot_sim_common - rmf_fleet_msgs - rclcpp - gazebo_ros - geometry_msgs - rmf_building_map_msgs -) - -target_include_directories(slotcar - PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIRS} - ${GAZEBO_INCLUDE_DIRS} -) - -############################### -# Install Targets -############################### - -install( - TARGETS - teleport_dispenser - teleport_ingestor - readonly - slotcar - LIBRARY DESTINATION lib/${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/rmf_robot_sim_gz_classic_plugins/QUALITY_DECLARATION.md b/rmf_robot_sim_gz_classic_plugins/QUALITY_DECLARATION.md deleted file mode 100644 index 89fe1e7..0000000 --- a/rmf_robot_sim_gz_classic_plugins/QUALITY_DECLARATION.md +++ /dev/null @@ -1,168 +0,0 @@ -This document is a declaration of software quality for the `rmf_robot_sim_gz_classic_plugins` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). - -# `rmf_robot_sim_gz_classic_plugins` Quality Declaration - -This package claims to be in the **Quality Level 4** category. - -Below are the detailed rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 4 in REP-2004](https://www.ros.org/reps/rep-2004.html). - -## Version Policy [1] - -### Version Scheme [1.i] - -This package uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). - -### Version Stability [1.ii] - -This package is at a stable version, i.e. `>= 1.0.0`. -The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). - -### Public API Declaration [1.iii] - -This package does not have a public API. - -### API Stability Policy [1.iv] - -This package does not have a public API. - -### ABI Stability Policy [1.v] - -This package does not have a public API. - -### API and ABI Stability Within a Released ROS Distribution [1.vi] - -This package does not have a public API. - -## Change Control Process [2] - -This package follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-requirements). - -### Change Requests [2.i] - -This package requires that all changes occur through a pull request. - -### Contributor Origin [2.ii] - -This package uses DCO as its confirmation of contributor origin policy. -More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). - -### Peer Review Policy [2.iii] - -All pull requests must have at least 1 peer review. - -### Continuous Integration [2.iv] - -All pull requests must pass CI on all platforms supported by RMF. -The CI checks only that the package builds. -The most recent CI results can be seen on [the workflow page](https://github.com/open-rmf/rmf_simulation/actions). - -### Documentation Policy [2.v] - -All pull requests must resolve related documentation changes before merging. - -## Documentation [3] - -### Feature Documentation [3.i] - -This package is documented in the [parent repository's README.md file](https://github.com/open-rmf/rmf_simulation/blob/master/README.md). - -### Public API Documentation [3.ii] - -This package does not have a public API. - -### License [3.iii] - -The license for this package is Apache 2.0, the type is declared in the [package.xml](package.xml) manifest file, and a full copy of the license is in the repository level [LICENSE](../LICENSE) file. - -### Copyright Statement [3.iv] - -The copyright holders each provide a statement of copyright in each source code file in this package. - -### Quality declaration document [3.v] - -This quality declaration is linked in the [README file](README.md). - -This quality declaration has not been externally peer-reviewed and is not registered on any Level 4 lists. - -## Testing [4] - -### Feature Testing [4.i] - -This package does not have any tests. - -### Public API Testing [4.ii] - -This package does not have a public API. - -### Coverage [4.iii] - -This package does not track coverage statistics. - -### Performance [4.iv] - -This package does not have performance tests. - -### Linters and Static Analysis [4.v] - -This package does not use the standard linters and static analysis tools for its CMake code to ensure it follows the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters). - -## Dependencies [5] - -### Direct Runtime ROS Dependencies [5.i] - -This package has the following direct runtime ROS dependencies. - -#### rclcpp - -`rclcpp` is [**Quality Level 1**](https://github.com/ros2/rclcpp/blob/master/rclcpp/QUALITY_DECLARATION.md). - -#### gazebo\_dev - -`gazebo_dev` does not declare a Quality Level. -It is assumed to be at least **Quality Level 4** based on its widespread use. - - -#### gazebo\_ros - -`gazebo_ros` does not declare a Quality Level. -It is assumed to be at least **Quality Level 4** based on its widespread use. - -#### rmf\_robot\_sim\_common - -`rmf_robot_sim_common` is [**Quality Level 4**](https://github.com/open-rmf/rmf_simulation/blob/main/rmf_robot_sim_common/QUALITY_DECLARATION.md). - -#### rmf\_fleet\_msgs - -`rmf_fleet_msgs` is [**Quality Level 3**](https://github.com/open-rmf/rmf_internal_msgs/blob/main/rmf_fleet_msgs/QUALITY_DECLARATION.md). - -#### rmf\_building\_map\_msgs - -`rmf_building_map_msgs` is [**Quality Level 3**](https://github.com/open-rmf/rmf_building_map_msgs/blob/main/rmf_building_map_msgs/QUALITY_DECLARATION.md). - -#### geometry\_msgs - -`gemoetry_msgs` is [**Quality Level 1**](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/QUALITY_DECLARATION.md). - -### Optional Direct Runtime ROS Dependencies [5.ii] - -This package does not have any optional direct runtime ROS dependencies. - -### Direct Runtime non-ROS Dependency [5.iii] - -This package has the following runtime non-ROS dependencies. - -#### eigen - -`eigen` does not declare a quality level. -It is assumed to be QL1 based on wide-spread use. - -## Platform Support [6] - -This package does not support all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers). -This package supports ROS Foxy. - -## Security [7] - -### Vulnerability Disclosure Policy [7.i] - -This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rmf_robot_sim_gz_classic_plugins/README.md b/rmf_robot_sim_gz_classic_plugins/README.md deleted file mode 100644 index 5021d08..0000000 --- a/rmf_robot_sim_gz_classic_plugins/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# rmf\_robot\_sim\_gz_classic\_plugins - -![](https://github.com/open-rmf/rmf_simulation/workflows/build/badge.svg) -![](https://github.com/open-rmf/rmf_simulation/workflows/asan/badge.svg) -![](https://github.com/open-rmf/rmf_simulation/workflows/tsan/badge.svg) -![](https://github.com/open-rmf/rmf_simulation/workflows/style/badge.svg) - -This package provides Gazebo-classic simulator plugins for the robots used in the RMF demos. - -## Quality Declaration - -This package claims to be in the **Quality Level 4** category, see the [Quality Declaration](./QUALITY_DECLARATION.md) for more details. diff --git a/rmf_robot_sim_gz_classic_plugins/package.xml b/rmf_robot_sim_gz_classic_plugins/package.xml deleted file mode 100644 index 70b7c18..0000000 --- a/rmf_robot_sim_gz_classic_plugins/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rmf_robot_sim_gz_classic_plugins - 2.2.2 - - ROS 2 Gazebo-classic plugins for TeleportIngestors, TeleportDispensers and Readonly Objects - - Luca Della Vedova - Yadunund - Aaron Chong - Rushyendra Maganty - Luca Della Vedova - Marco A. Gutiérrez - Apache 2.0 - - ament_cmake - - rclcpp - gazebo_dev - gazebo_ros - eigen - geometry_msgs - rmf_fleet_msgs - rmf_building_map_msgs - rmf_robot_sim_common - - - - ament_cmake - - - diff --git a/rmf_robot_sim_gz_classic_plugins/src/TeleportDispenser.cpp b/rmf_robot_sim_gz_classic_plugins/src/TeleportDispenser.cpp deleted file mode 100644 index ed261e2..0000000 --- a/rmf_robot_sim_gz_classic_plugins/src/TeleportDispenser.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/* - * Copyright (C) 2020 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 -#include -#include -#include - -#include - -#include - -#include -#include - -using namespace rmf_dispenser_common; -using namespace rmf_plugins_utils; - -namespace rmf_robot_sim_gz_classic_plugins { - -class TeleportDispenserPlugin : public gazebo::ModelPlugin -{ - -public: - using FleetState = rmf_fleet_msgs::msg::FleetState; - using FleetStateIt = - std::unordered_map::iterator; - - TeleportDispenserPlugin(); - ~TeleportDispenserPlugin(); - void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf) override; - -private: - // Stores params representing state of Dispenser, and handles all message pub/sub - std::unique_ptr _dispenser_common; - - gazebo::event::ConnectionPtr _update_connection; - gazebo::physics::ModelPtr _model; - gazebo::physics::ModelPtr _item_model; // Item that dispenser may contain - gazebo::physics::WorldPtr _world; - - #if GAZEBO_MAJOR_VERSION <= 9 - ignition::math::Box _dispenser_vicinity_box; - #else - ignition::math::AxisAlignedBox _dispenser_vicinity_box; - #endif - - SimEntity find_nearest_model( - const std::vector& models, - bool& found) const; - void place_on_entity(const SimEntity& to_move); - void fill_robot_list(FleetStateIt fleet_state_it, - std::vector& robot_model_list); - void fill_dispenser(); - void create_dispenser_bounding_box(); - void on_update(); -}; - -SimEntity TeleportDispenserPlugin::find_nearest_model( - const std::vector& models, - bool& found) const -{ - double nearest_dist = 1e6; - SimEntity nearest_model(""); - - for (const SimEntity& sim_obj : models) - { - if (sim_obj.get_name() == _dispenser_common->guid) - continue; - - gazebo::physics::EntityPtr m = _world->EntityByName(sim_obj.get_name()); - const double dist = - m->WorldPose().Pos().Distance(_model->WorldPose().Pos()); - if (dist < nearest_dist) - { - nearest_dist = dist; - nearest_model = sim_obj; - found = true; - } - } - return nearest_model; -} - -void TeleportDispenserPlugin::place_on_entity(const SimEntity& to_move) -{ - _item_model->PlaceOnEntity(to_move.get_name()); -} - -void TeleportDispenserPlugin::fill_robot_list(FleetStateIt fleet_state_it, - std::vector& robot_model_list) -{ - for (const auto& rs : fleet_state_it->second->robots) - { - const auto r_model = _world->ModelByName(rs.name); - if (r_model && !r_model->IsStatic()) - robot_model_list.push_back(SimEntity(r_model->GetName())); - } -} - -// Searches vicinity of Dispenser for closest valid item. Assigns _item_model to the newly found item -void TeleportDispenserPlugin::fill_dispenser() -{ - auto model_list = _world->Models(); - double nearest_dist = 1.0; - const auto dispenser_pos = _model->WorldPose().Pos(); - for (const auto& m : model_list) - { - if (!m || m->IsStatic() || m->GetName() == _model->GetName()) - continue; - - const double dist = m->WorldPose().Pos().Distance(dispenser_pos); - if (dist < nearest_dist && - _dispenser_vicinity_box.Intersects(m->BoundingBox())) - { - _item_model = m; - nearest_dist = dist; - _dispenser_common->dispenser_filled = true; - _dispenser_common->item_en_found = true; - } - } -} - -void TeleportDispenserPlugin::create_dispenser_bounding_box() -{ - const auto dispenser_pos = _model->WorldPose().Pos(); - ignition::math::Vector3d corner_1(dispenser_pos.X() - 0.05, - dispenser_pos.Y() - 0.05, dispenser_pos.Z() - 0.05); - ignition::math::Vector3d corner_2(dispenser_pos.X() + 0.05, - dispenser_pos.Y() + 0.05, dispenser_pos.Z() + 0.05); - #if GAZEBO_MAJOR_VERSION <= 9 - _dispenser_vicinity_box = ignition::math::Box(corner_1, corner_2); - #else - _dispenser_vicinity_box = - ignition::math::AxisAlignedBox(corner_1, corner_2); - #endif -} - -void TeleportDispenserPlugin::on_update() -{ - _dispenser_common->sim_time = _world->SimTime().Double(); - - std::function&)> fill_robot_list_cb = - std::bind(&TeleportDispenserPlugin::fill_robot_list, this, - std::placeholders::_1, std::placeholders::_2); - - std::function&, - bool&)> find_nearest_model_cb = - std::bind(&TeleportDispenserPlugin::find_nearest_model, this, - std::placeholders::_1, std::placeholders::_2); - - std::function place_on_entity_cb = - std::bind(&TeleportDispenserPlugin::place_on_entity, this, - std::placeholders::_1); - - std::function check_filled_cb = [&]() - { - return _dispenser_vicinity_box.Contains(_item_model->WorldPose().Pos()); - }; - - _dispenser_common->on_update(fill_robot_list_cb, find_nearest_model_cb, - place_on_entity_cb, check_filled_cb); -} - -TeleportDispenserPlugin::TeleportDispenserPlugin() -: _dispenser_common(std::make_unique()) -{ -} - -TeleportDispenserPlugin::~TeleportDispenserPlugin() -{ - rclcpp::shutdown(); -} - -void TeleportDispenserPlugin::Load(gazebo::physics::ModelPtr _parent, - sdf::ElementPtr _sdf) -{ - _model = _parent; - _world = _model->GetWorld(); - _dispenser_common->guid = _model->GetName(); - const std::string& node_name = _model->GetName(); - - _dispenser_common->init_ros_node( - gazebo_ros::Node::Get(_sdf, node_name)); - RCLCPP_INFO( - _dispenser_common->ros_node->get_logger(), - "Started TeleportDispenserPlugin node..."); - - create_dispenser_bounding_box(); - fill_dispenser(); - - if (!_item_model) - { - RCLCPP_WARN(_dispenser_common->ros_node->get_logger(), - "Could not find dispenser item model within 1 meter, " - "this dispenser will not be operational"); - return; - } - - RCLCPP_INFO(_dispenser_common->ros_node->get_logger(), - "Found dispenser item: [%s]", _item_model->GetName().c_str()); - - _update_connection = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&TeleportDispenserPlugin::on_update, this)); -} - -} // namespace rmf_robot_sim_gz_classic_plugins - -GZ_REGISTER_MODEL_PLUGIN( - rmf_robot_sim_gz_classic_plugins::TeleportDispenserPlugin) diff --git a/rmf_robot_sim_gz_classic_plugins/src/TeleportIngestor.cpp b/rmf_robot_sim_gz_classic_plugins/src/TeleportIngestor.cpp deleted file mode 100644 index 7b50ce7..0000000 --- a/rmf_robot_sim_gz_classic_plugins/src/TeleportIngestor.cpp +++ /dev/null @@ -1,244 +0,0 @@ -/* - * Copyright (C) 2020 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 -#include -#include -#include -#include - -#include -#include - -#include -#include - -using namespace rmf_plugins_utils; -using namespace rmf_ingestor_common; - -namespace rmf_robot_sim_gz_classic_plugins { - -class TeleportIngestorPlugin : public gazebo::ModelPlugin -{ - -public: - using FleetState = rmf_fleet_msgs::msg::FleetState; - using FleetStateIt = - std::unordered_map::iterator; - - TeleportIngestorPlugin(); - ~TeleportIngestorPlugin(); - void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf) override; - -private: - // Stores params representing state of Ingestor, and handles all message pub/sub - std::unique_ptr _ingestor_common; - - gazebo::event::ConnectionPtr _update_connection; - gazebo::physics::ModelPtr _model; - gazebo::physics::ModelPtr _ingested_model; // Item that ingestor may contain - gazebo::physics::WorldPtr _world; - - SimEntity find_nearest_model(const std::vector& models, - bool& found) const; - bool get_payload_model(const SimEntity& robot_model, - gazebo::physics::ModelPtr& payload_model) const; - void fill_robot_list(FleetStateIt fleet_state_it, - std::vector& robot_model_list); - void transport_model(); - void send_ingested_item_home(); - void on_update(); -}; - -SimEntity TeleportIngestorPlugin::find_nearest_model( - const std::vector& models, - bool& found) const -{ - double nearest_dist = 1e6; - SimEntity nearest_model(""); - - for (const SimEntity& sim_obj : models) - { - if (sim_obj.get_name() == _model->GetName()) - continue; - - // If `models` has been generated with `fill_robot_list`, it is - // guaranteed to have a valid name field - gazebo::physics::EntityPtr m = _world->EntityByName(sim_obj.get_name()); - const double dist = - m->WorldPose().Pos().Distance(_model->WorldPose().Pos()); - if (dist < nearest_dist) - { - nearest_dist = dist; - nearest_model = sim_obj; - found = true; - } - } - return nearest_model; -} - -// Identifies item to ingest and assigns it to `payload_model` -bool TeleportIngestorPlugin::get_payload_model( - const SimEntity& robot_sim_entity, - gazebo::physics::ModelPtr& payload_model) const -{ - const auto robot_model = _world->EntityByName(robot_sim_entity.get_name()); - const auto robot_collision_bb = robot_model->BoundingBox(); - ignition::math::Vector3d max_corner = robot_collision_bb.Max(); - - // Create a new bounding box extended slightly in the Z direction - max_corner.Z(max_corner.Z() + 0.1); - #if GAZEBO_MAJOR_VERSION <= 9 - const ignition::math::Box vicinity_box( - robot_collision_bb.Min(), max_corner); - #else - const ignition::math::AxisAlignedBox vicinity_box( - robot_collision_bb.Min(), max_corner); - #endif - - // There might not be a better way to loop through all the models, as we - // might consider delivering items that were spawned during run time, - // instead of during launch. - const auto robot_model_pos = robot_model->WorldPose().Pos(); - double nearest_dist = 1.0; - const auto model_list = _world->Models(); - bool found = false; - for (const auto& m : model_list) - { - if (!m || m->IsStatic() || - m->GetName() == _model->GetName() || - m->GetName() == robot_model->GetName()) - continue; - - const double dist = m->WorldPose().Pos().Distance(robot_model_pos); - if (dist < nearest_dist && vicinity_box.Intersects(m->BoundingBox())) - { - payload_model = m; - nearest_dist = dist; - found = true; - } - } - return found; -} - -void TeleportIngestorPlugin::fill_robot_list( - FleetStateIt fleet_state_it, std::vector& robot_model_list) -{ - for (const auto& rs : fleet_state_it->second->robots) - { - const auto r_model = _world->ModelByName(rs.name); - if (r_model && !r_model->IsStatic()) - robot_model_list.push_back(SimEntity(r_model->GetName())); - } -} - -// Moves the identified item to ingest from its current position to the ingestor -void TeleportIngestorPlugin::transport_model() -{ - _ingested_model->SetWorldPose(_model->WorldPose()); -} - -void TeleportIngestorPlugin::send_ingested_item_home() -{ - if (_ingestor_common->ingestor_filled) - { - const auto it = _ingestor_common->non_static_models_init_poses.find( - _ingested_model->GetName()); - if (it == _ingestor_common->non_static_models_init_poses.end()) - _world->RemoveModel(_ingested_model); - else - _ingested_model->SetWorldPose(convert_to_pose( - it->second)); - - _ingestor_common->ingestor_filled = false; // Assumes ingestor can only hold 1 object at a time - } -} - -void TeleportIngestorPlugin::on_update() -{ - _ingestor_common->sim_time = _world->SimTime().Double(); - - std::function&)> fill_robot_list_cb = - std::bind(&TeleportIngestorPlugin::fill_robot_list, this, - std::placeholders::_1, std::placeholders::_2); - - std::function&, - bool&)> find_nearest_model_cb = - std::bind(&TeleportIngestorPlugin::find_nearest_model, this, - std::placeholders::_1, std::placeholders::_2); - - std::function get_payload_model_cb = - std::bind(&TeleportIngestorPlugin::get_payload_model, this, - std::placeholders::_1, std::ref(_ingested_model)); - - std::function transport_model_cb = - std::bind(&TeleportIngestorPlugin::transport_model, this); - - std::function send_ingested_item_home_cb = - std::bind(&TeleportIngestorPlugin::send_ingested_item_home, this); - - _ingestor_common->on_update(fill_robot_list_cb, find_nearest_model_cb, - get_payload_model_cb, transport_model_cb, send_ingested_item_home_cb); -} - -void TeleportIngestorPlugin::Load(gazebo::physics::ModelPtr _parent, - sdf::ElementPtr _sdf) -{ - _model = _parent; - _world = _model->GetWorld(); - _ingested_model = nullptr; - - _ingestor_common->_guid = _model->GetName(); - const std::string& node_name = _model->GetName() + "_node"; - _ingestor_common->init_ros_node(gazebo_ros::Node::Get(_sdf, node_name)); - RCLCPP_INFO(_ingestor_common->ros_node->get_logger(), - "Started TeleportIngestorPlugin node..."); - - // Keep track of all the non-static models - auto model_list = _world->Models(); - for (const auto& m : model_list) - { - std::string m_name = m->GetName(); - if (m && !(m->IsStatic()) && m_name != _model->GetName()) - { - _ingestor_common->non_static_models_init_poses[m_name] = convert_pose( - m->WorldPose()); - } - } - - _update_connection = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&TeleportIngestorPlugin::on_update, this)); -} - -TeleportIngestorPlugin::TeleportIngestorPlugin() -: _ingestor_common(std::make_unique()) -{ -} - -TeleportIngestorPlugin::~TeleportIngestorPlugin() -{ - rclcpp::shutdown(); -} - -} // namespace rmf_robot_sim_gz_classic_plugins - -GZ_REGISTER_MODEL_PLUGIN( - rmf_robot_sim_gz_classic_plugins::TeleportIngestorPlugin) diff --git a/rmf_robot_sim_gz_classic_plugins/src/readonly.cpp b/rmf_robot_sim_gz_classic_plugins/src/readonly.cpp deleted file mode 100644 index e5f84bb..0000000 --- a/rmf_robot_sim_gz_classic_plugins/src/readonly.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright (C) 2020 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 -#include - -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include - -class ReadonlyPlugin : public gazebo::ModelPlugin -{ -public: - ReadonlyPlugin(); - void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; - void OnUpdate(); - -private: - std::unique_ptr _readonly_common; - gazebo::event::ConnectionPtr _update_connection; - gazebo::physics::ModelPtr _model; -}; - -ReadonlyPlugin::ReadonlyPlugin() -: _readonly_common(std::make_unique()) -{ -} - -void ReadonlyPlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) -{ - _model = model; - _readonly_common->set_name(_model->GetName()); - _readonly_common->read_sdf(sdf); - const std::string& node_name = _model->GetName() + "_node"; - _readonly_common->init(gazebo_ros::Node::Get(sdf, node_name)); - - _update_connection = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&ReadonlyPlugin::OnUpdate, this)); -} - -void ReadonlyPlugin::OnUpdate() -{ - const auto& world = _model->GetWorld(); - auto pose = rmf_plugins_utils::convert_pose(_model->WorldPose()); - auto sim_time = world->SimTime().Double(); - _readonly_common->on_update(pose, sim_time); -} - -GZ_REGISTER_MODEL_PLUGIN(ReadonlyPlugin) diff --git a/rmf_robot_sim_gz_classic_plugins/src/slotcar.cpp b/rmf_robot_sim_gz_classic_plugins/src/slotcar.cpp deleted file mode 100644 index 702ebd0..0000000 --- a/rmf_robot_sim_gz_classic_plugins/src/slotcar.cpp +++ /dev/null @@ -1,187 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -class SlotcarPlugin : public gazebo::ModelPlugin -{ -public: - SlotcarPlugin(); - ~SlotcarPlugin(); - - void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override; - void OnUpdate(); - -private: - std::unique_ptr dataPtr; - - gazebo::transport::NodePtr _gazebo_node; - gazebo::transport::SubscriberPtr _charge_state_sub; - - gazebo::event::ConnectionPtr _update_connection; - gazebo::physics::ModelPtr _model; - - std::array joints; - - std::unordered_set obstacle_exclusions; - - // Book keeping - double last_update_time = 0.0; - - void init_obstacle_exclusions(); - - std::vector get_obstacle_positions( - const gazebo::physics::WorldPtr& world); - - void charge_state_cb(ConstSelectionPtr& msg); - - void send_control_signals(const std::pair& displacements, - const double dt, const double target_linear_speed_now, - const double target_linear_speed_destination, - const std::optional& max_linear_velocity) - { - std::array w_tire; - for (std::size_t i = 0; i < 2; ++i) - w_tire[i] = joints[i]->GetVelocity(0); - auto joint_signals = dataPtr->calculate_joint_control_signals(w_tire, - displacements, dt, target_linear_speed_now, - target_linear_speed_destination, - max_linear_velocity); - for (std::size_t i = 0; i < 2; ++i) - { - joints[i]->SetParam("vel", 0, joint_signals[i]); - joints[i]->SetParam("fmax", 0, 10000000.0); // TODO(MXG): Replace with realistic torque limit - } - } -}; - -SlotcarPlugin::SlotcarPlugin() -: dataPtr(std::make_unique()) -{ - // Listen for messages that enable/disable charging - _gazebo_node = gazebo::transport::NodePtr(new gazebo::transport::Node()); - _gazebo_node->Init(); - _charge_state_sub = _gazebo_node->Subscribe("/charge_state", - &SlotcarPlugin::charge_state_cb, this); - // We do rest of initialization during ::Load -} - -SlotcarPlugin::~SlotcarPlugin() -{ -} - -void SlotcarPlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) -{ - _model = model; - dataPtr->set_model_name(_model->GetName()); - dataPtr->read_sdf(sdf); - std::string node_name = _model->GetName() + "_node"; - rmf_plugins_utils::sanitize_node_name(node_name); - gazebo_ros::Node::SharedPtr _ros_node = gazebo_ros::Node::Get(sdf, node_name); - dataPtr->init_ros_node(_ros_node); - - RCLCPP_INFO( - dataPtr->logger(), - "Initialising slotcar for %s", - model->GetName().c_str()); - - _update_connection = gazebo::event::Events::ConnectWorldUpdateBegin( - std::bind(&SlotcarPlugin::OnUpdate, this)); - - joints[0] = _model->GetJoint("joint_tire_left"); - if (!joints[0]) - { - RCLCPP_ERROR( - dataPtr->logger(), - "Could not find tire for [joint_tire_left]"); - } - - joints[1] = _model->GetJoint("joint_tire_right"); - if (!joints[1]) - { - RCLCPP_ERROR( - dataPtr->logger(), - "Could not find tire for [joint_tire_right]"); - } -} - -void SlotcarPlugin::charge_state_cb(ConstSelectionPtr& msg) -{ - dataPtr->charge_state_cb(msg->name(), msg->selected()); -} - -void SlotcarPlugin::init_obstacle_exclusions() -{ - const auto& world = _model->GetWorld(); - obstacle_exclusions.insert(_model.get()); - const auto& all_models = world->Models(); - for (const auto& m : all_models) - { - // Object should not be static, be part of infrastructure, or dispensable - if (!m->IsStatic()) - { - std::string name = m->GetName(); - std::for_each(name.begin(), name.end(), [](char& c) - { - c = ::tolower(c); - }); - if (name.find("door") != std::string::npos || - name.find("lift") != std::string::npos || - name.find("dispensable") != std::string::npos) - obstacle_exclusions.insert(m.get()); - } - } -} - -std::vector SlotcarPlugin::get_obstacle_positions( - const gazebo::physics::WorldPtr& world) -{ - std::vector obstacle_positions; - - for (const auto& m : world->Models()) - { - // Object should not be static, not part of obstacle_exclusions, - // and close than a threshold (checked by common function) - const auto p_obstacle = m->WorldPose().Pos(); - if (m->IsStatic() == false && - obstacle_exclusions.find(m.get()) == obstacle_exclusions.end()) - obstacle_positions.push_back(rmf_plugins_utils::convert_vec(p_obstacle)); - } - - return obstacle_positions; -} - -void SlotcarPlugin::OnUpdate() -{ - const auto& world = _model->GetWorld(); - - // After initialization once, this set will have at least one exclusion, which - // is the itself. - if (obstacle_exclusions.empty()) - init_obstacle_exclusions(); - - const double time = world->SimTime().Double(); - const double dt = time - last_update_time; - last_update_time = time; - - auto pose = _model->WorldPose(); - auto obstacle_positions = get_obstacle_positions(world); - - auto update_result = - dataPtr->update(rmf_plugins_utils::convert_pose(pose), - obstacle_positions, time); - - send_control_signals({update_result.v, update_result.w}, dt, - update_result.target_linear_speed_now, - update_result.target_linear_speed_destination, - update_result.max_speed); -} - -GZ_REGISTER_MODEL_PLUGIN(SlotcarPlugin) diff --git a/rmf_robot_sim_gz_plugins/CMakeLists.txt b/rmf_robot_sim_gz_plugins/CMakeLists.txt index 352806f..5bff7aa 100644 --- a/rmf_robot_sim_gz_plugins/CMakeLists.txt +++ b/rmf_robot_sim_gz_plugins/CMakeLists.txt @@ -22,30 +22,24 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(ignition-cmake2 REQUIRED) - -ign_find_package(ignition-gazebo6 REQUIRED) -set(IGN_GAZEBO_VER 6) -ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) -set(IGN_PLUGIN_VER 1) -ign_find_package(ignition-common4 REQUIRED) -set(IGN_COMMON_VER 4) -ign_find_package(ignition-math6 REQUIRED) -set(IGN_MATH_VER 6) -ign_find_package(ignition-gui6 REQUIRED) -set(IGN_GUI_VER 6) -ign_find_package(ignition-msgs8 REQUIRED) -set(IGN_MSGS_VER 8) -ign_find_package(ignition-transport11 REQUIRED) -set(IGN_TRANSPORT_VER 11) -ign_find_package(ignition-rendering6 REQUIRED) -set(IGN_RENDERING_VER 6) -ign_find_package(sdformat12 REQUIRED) +find_package(gz_sim_vendor REQUIRED) +find_package(gz-sim REQUIRED) +find_package(gz_plugin_vendor REQUIRED) +find_package(gz-plugin REQUIRED) +find_package(gz_gui_vendor REQUIRED) +find_package(gz-gui REQUIRED) +find_package(gz_msgs_vendor REQUIRED) +find_package(gz-msgs REQUIRED) +find_package(gz_transport_vendor REQUIRED) +find_package(gz-transport REQUIRED) +find_package(gz_rendering_vendor REQUIRED) +find_package(gz-rendering REQUIRED) find_package(Eigen3 REQUIRED) find_package(rmf_fleet_msgs REQUIRED) find_package(rmf_building_map_msgs REQUIRED) find_package(rmf_robot_sim_common REQUIRED) +find_package(rmf_building_sim_gz_plugins REQUIRED) find_package (Qt5 COMPONENTS Core @@ -62,11 +56,14 @@ include(GNUInstallDirs) add_library(teleport_ingestor SHARED src/TeleportIngestor.cpp) +target_link_libraries(teleport_ingestor + gz-sim::core + gz-plugin::core + gz-msgs::core + gz-transport::core +) + ament_target_dependencies(teleport_ingestor - ignition-gazebo${IGN_GAZEBO_VER} - ignition-plugin${IGN_PLUGIN_VER} - ignition-msgs${IGN_MSGS_VER} - ignition-transport${IGN_TRANSPORT_VER} rmf_fleet_msgs rclcpp rmf_robot_sim_common @@ -86,11 +83,14 @@ target_include_directories(teleport_ingestor add_library(teleport_dispenser SHARED src/TeleportDispenser.cpp) +target_link_libraries(teleport_dispenser + gz-sim::core + gz-plugin::core + gz-msgs::core + gz-transport::core +) + ament_target_dependencies(teleport_dispenser - ignition-gazebo${IGN_GAZEBO_VER} - ignition-plugin${IGN_PLUGIN_VER} - ignition-msgs${IGN_MSGS_VER} - ignition-transport${IGN_TRANSPORT_VER} rmf_fleet_msgs rclcpp rmf_robot_sim_common @@ -110,12 +110,15 @@ target_include_directories(teleport_dispenser add_library(readonly SHARED ${PROJECT_SOURCE_DIR}/src/readonly.cpp) +target_link_libraries(readonly + gz-sim::core + gz-plugin::core +) + ament_target_dependencies(readonly rmf_fleet_msgs rmf_building_map_msgs rclcpp - ignition-gazebo${IGN_GAZEBO_VER} - ignition-plugin${IGN_PLUGIN_VER} rmf_robot_sim_common Eigen3 ) @@ -133,13 +136,16 @@ target_include_directories(readonly add_library(slotcar SHARED ${PROJECT_SOURCE_DIR}/src/slotcar.cpp) +target_link_libraries(slotcar + gz-sim::core + gz-plugin::core +) + ament_target_dependencies(slotcar - PUBLIC - ignition-gazebo${IGN_GAZEBO_VER} - ignition-plugin${IGN_PLUGIN_VER} - rmf_robot_sim_common - rmf_fleet_msgs - rclcpp + rmf_robot_sim_common + rmf_building_sim_gz_plugins + rmf_fleet_msgs + rclcpp ) target_include_directories(slotcar @@ -149,6 +155,7 @@ target_include_directories(slotcar ${rmf_robot_sim_common_INCLUDE_DIRS} ${rmf_fleet_msgs_INCLUDE_DIRS} ${rmf_building_map_msgs_INCLUDE_DIRS} + ${rmf_building_sim_gz_plugins_INCLUDE_DIRS} ) ############################### @@ -162,12 +169,15 @@ add_library(LightTuning SHARED ${headers_MOC} ${resources_RCC} ) +target_link_libraries(LightTuning + gz-gui::core + gz-sim::core + gz-msgs::core + gz-transport::core + gz-rendering::core +) + ament_target_dependencies(LightTuning - ignition-gui${IGN_GUI_VER} - ignition-gazebo${IGN_GAZEBO_VER} - ignition-msgs${IGN_MSGS_VER} - ignition-transport${IGN_TRANSPORT_VER} - ignition-rendering${IGN_RENDERING_VER} Qt5Core Qt5Qml Qt5Quick diff --git a/rmf_robot_sim_gz_plugins/package.xml b/rmf_robot_sim_gz_plugins/package.xml index 3eadbb2..693d0b0 100644 --- a/rmf_robot_sim_gz_plugins/package.xml +++ b/rmf_robot_sim_gz_plugins/package.xml @@ -19,10 +19,16 @@ rmf_fleet_msgs rmf_building_map_msgs rmf_robot_sim_common + rmf_building_sim_gz_plugins libqt5-core libqt5-qml libqt5-quick - ignition-fortress + gz_gui_vendor + gz_msgs_vendor + gz_plugin_vendor + gz_rendering_vendor + gz_sim_vendor + gz_transport_vendor ament_cmake diff --git a/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp index 0b23f05..566113c 100644 --- a/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp +++ b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp @@ -19,28 +19,28 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -#include -#include -#include +#include +#include +#include -#include +#include -#include -#include -#include +#include +#include +#include // Helper function that creates a simple cube sdf model string std::string create_light_marker_str(const std::string& name, - const ignition::math::Pose3d& pose) + const gz::math::Pose3d& pose) { std::ostringstream ss; ss << std::string( @@ -86,14 +86,14 @@ std::optional parse_light_type(const std::string& type) return std::nullopt; } -std::optional parse_pose(const std::string& pose_str) +std::optional parse_pose(const std::string& pose_str) { std::stringstream ss(pose_str); double x, y, z, roll, pitch, yaw; ss >> x >> y >> z >> roll >> pitch >> yaw; if (!ss.fail()) { - return ignition::math::Pose3d(x, y, z, roll, pitch, yaw); + return gz::math::Pose3d(x, y, z, roll, pitch, yaw); } else { @@ -103,14 +103,14 @@ std::optional parse_pose(const std::string& pose_str) } } -std::optional parse_color(const std::string& color_str) +std::optional parse_color(const std::string& color_str) { std::stringstream ss(color_str); float r, g, b, a; ss >> r >> g >> b >> a; if (!ss.fail()) { - return ignition::math::Color(r, g, b, a); + return gz::math::Color(r, g, b, a); } else { @@ -137,7 +137,7 @@ std::optional parse_double(const std::string& double_str) } } -std::optional parse_vector( +std::optional parse_vector( const std::string& vector_str) { std::stringstream ss(vector_str); @@ -145,7 +145,7 @@ std::optional parse_vector( ss >> x >> y >> z; if (!ss.fail()) { - return ignition::math::Vector3d(x, y, z); + return gz::math::Vector3d(x, y, z); } else { @@ -155,7 +155,7 @@ std::optional parse_vector( } } -std::optional parse_angle( +std::optional parse_angle( const std::string& angle_str) { std::stringstream ss(angle_str); @@ -163,7 +163,7 @@ std::optional parse_angle( ss >> angle; if (!ss.fail()) { - return ignition::math::Angle(angle); + return gz::math::Angle(angle); } else { @@ -258,7 +258,7 @@ class LightsModel : public QAbstractListModel const QVector& get_lights() const; // Fill _existing_names with names from the ecm - void populate_names(ignition::gazebo::EntityComponentManager& ecm); + void populate_names(gz::sim::EntityComponentManager& ecm); private: // Collection of lights, each with a unique name (enforced by @@ -420,11 +420,11 @@ const QVector& LightsModel::get_lights() const return _lights; } -void LightsModel::populate_names(ignition::gazebo::EntityComponentManager& ecm) +void LightsModel::populate_names(gz::sim::EntityComponentManager& ecm) { - ecm.Each( - [&](const ignition::gazebo::Entity&, - const ignition::gazebo::components::Name* name) + ecm.Each( + [&](const gz::sim::Entity&, + const gz::sim::components::Name* name) -> bool { _pre_existing_names.insert(name->Data()); @@ -434,7 +434,7 @@ void LightsModel::populate_names(ignition::gazebo::EntityComponentManager& ecm) // Class that handles all GUI interactions and their associated // light creations/deletions -class LightTuning : public ignition::gazebo::GuiSystem +class LightTuning : public gz::sim::GuiSystem { Q_OBJECT @@ -442,8 +442,8 @@ class LightTuning : public ignition::gazebo::GuiSystem virtual void LoadConfig(const tinyxml2::XMLElement* _pluginElem) override; - void Update(const ignition::gazebo::UpdateInfo& _info, - ignition::gazebo::EntityComponentManager& _ecm) override; + void Update(const gz::sim::UpdateInfo& _info, + gz::sim::EntityComponentManager& _ecm) override; signals: void poseChanged(QString nm, QString new_pose); @@ -476,9 +476,9 @@ protected: bool eventFilter(QObject* _obj, QEvent* _event) override; const std::string sdf_close_tag = ""; std::string _world_name; - ignition::transport::Node _node; + gz::transport::Node _node; LightsModel _model; - ignition::rendering::ScenePtr scene_ptr; + gz::rendering::ScenePtr scene_ptr; // Contains an Entity that serves as a physical representation of // the light on the screen, so that a user can move it around to set the @@ -486,8 +486,8 @@ protected: bool eventFilter(QObject* _obj, QEvent* _event) override; struct LightMarker { std::string name; // Name of the LightMarker - ignition::gazebo::Entity en; - ignition::math::Pose3d last_set_pose; + gz::sim::Entity en; + gz::math::Pose3d last_set_pose; }; // List of pairs of light name and corresponding marker name being spawned std::vector> _markers_spawn_pipeline; @@ -506,7 +506,7 @@ protected: bool eventFilter(QObject* _obj, QEvent* _event) override; // Sends a service request to render the LightMarker corresponding to the light // with name `light_name` using Ignition transport void create_marker_service( - const std::string& light_name, const ignition::math::Pose3d& pose); + const std::string& light_name, const gz::math::Pose3d& pose); // Sends a service request to remove the LightMarker corresponding to the light // with name `light_name` using Ignition transport void remove_marker_service(const std::string& light_name); @@ -527,27 +527,27 @@ void LightTuning::LoadConfig(const tinyxml2::XMLElement*) this->title = "Light Tuning"; // To monitor and intercept rendering and entity selection events - ignition::gui::App()->findChild< - ignition::gui::MainWindow*>()->installEventFilter(this); + gz::gui::App()->findChild< + gz::gui::MainWindow*>()->installEventFilter(this); // Connect data model to view this->Context()->setContextProperty( "LightsModel", &this->_model); } -void LightTuning::Update(const ignition::gazebo::UpdateInfo&, - ignition::gazebo::EntityComponentManager& ecm) +void LightTuning::Update(const gz::sim::UpdateInfo&, + gz::sim::EntityComponentManager& ecm) { if (first_update_call) { // Get world name for sending create/remove requests later. // Assumes there is only 1 world in the simulation - ecm.Each( - [&](const ignition::gazebo::Entity&, - const ignition::gazebo::components::World*, - const ignition::gazebo::components::Name* name) + ecm.Each( + [&](const gz::sim::Entity&, + const gz::sim::components::World*, + const gz::sim::components::Name* name) -> bool { _world_name = name->Data(); @@ -568,14 +568,14 @@ void LightTuning::Update(const ignition::gazebo::UpdateInfo&, std::string& marker_name = _new_markers_it->second; auto marker_en = ecm.EntityByComponents( - ignition::gazebo::components::Name(marker_name), - ignition::gazebo::components::Model()); - if (marker_en != ignition::gazebo::kNullEntity) + gz::sim::components::Name(marker_name), + gz::sim::components::Model()); + if (marker_en != gz::sim::kNullEntity) { auto pose_component = - ecm.Component(marker_en); + ecm.Component(marker_en); auto pose = - pose_component ? pose_component->Data() : ignition::math::Pose3d(); + pose_component ? pose_component->Data() : gz::math::Pose3d(); _markers[light_name] = LightMarker {marker_name, marker_en, pose}; _new_markers_it = _markers_spawn_pipeline.erase(_new_markers_it); } @@ -588,7 +588,7 @@ void LightTuning::Update(const ignition::gazebo::UpdateInfo&, bool LightTuning::eventFilter(QObject* _obj, QEvent* _event) { - if (_event->type() == ignition::gui::events::Render::kType) + if (_event->type() == gz::gui::events::Render::kType) { if (!scene_ptr) { @@ -599,13 +599,13 @@ bool LightTuning::eventFilter(QObject* _obj, QEvent* _event) render_lights(); } else if (_event->type() == - ignition::gazebo::gui::events::EntitiesSelected::kType) + gz::sim::gui::events::EntitiesSelected::kType) { auto event = - reinterpret_cast(_event); + reinterpret_cast(_event); if (event && !event->Data().empty()) { - const ignition::gazebo::Entity en = *event->Data().begin(); + const gz::sim::Entity en = *event->Data().begin(); auto it = std::find_if(_markers.begin(), _markers.end(), [&en](const std::pair& marker) { @@ -626,7 +626,7 @@ bool LightTuning::eventFilter(QObject* _obj, QEvent* _event) void LightTuning::load_scene() { - auto loadedEngNames = ignition::rendering::loadedEngines(); + auto loadedEngNames = gz::rendering::loadedEngines(); if (loadedEngNames.empty()) return; @@ -638,7 +638,7 @@ void LightTuning::load_scene() "Grid config plugin will use engine [" << engineName << "]" << std::endl; } - auto engine = ignition::rendering::engine(engineName); + auto engine = gz::rendering::engine(engineName); if (!engine) { ignerr << "Internal error: failed to load engine [" << engineName << @@ -690,7 +690,7 @@ void LightTuning::render_lights() auto marker_it = _markers.find(light_queue_it->first); if (marker_it == _markers.end()) { - create_marker_service(light_queue_it->first, ignition::math::Pose3d()); + create_marker_service(light_queue_it->first, gz::math::Pose3d()); } } else @@ -703,18 +703,18 @@ void LightTuning::render_lights() } } -sdf::LightType get_light_ptr_type(const ignition::rendering::LightPtr light_ptr) +sdf::LightType get_light_ptr_type(const gz::rendering::LightPtr light_ptr) { - if (std::dynamic_pointer_cast( + if (std::dynamic_pointer_cast( light_ptr)) { return sdf::LightType::DIRECTIONAL; } - else if (std::dynamic_pointer_cast(light_ptr)) + else if (std::dynamic_pointer_cast(light_ptr)) { return sdf::LightType::POINT; } - else if (std::dynamic_pointer_cast(light_ptr)) + else if (std::dynamic_pointer_cast(light_ptr)) { return sdf::LightType::SPOT; } @@ -766,7 +766,7 @@ void LightTuning::create_light_rendering(const std::string& name) case sdf::LightType::SPOT: { auto spot_light = - std::dynamic_pointer_cast(light_ptr); + std::dynamic_pointer_cast(light_ptr); spot_light->SetInnerAngle(light.SpotInnerAngle()); spot_light->SetOuterAngle(light.SpotOuterAngle()); spot_light->SetFalloff(light.SpotFalloff()); @@ -775,7 +775,7 @@ void LightTuning::create_light_rendering(const std::string& name) case sdf::LightType::DIRECTIONAL: { auto dir_light = - std::dynamic_pointer_cast( + std::dynamic_pointer_cast( light_ptr); dir_light->SetDirection(light.Direction()); break; @@ -816,16 +816,16 @@ std::string LightTuning::light_to_sdf_string(const sdf::Light& light) return ss.str(); } -void marker_service_cb(const ignition::msgs::Boolean&, const bool) +void marker_service_cb(const gz::msgs::Boolean&, const bool) { } void LightTuning::create_marker_service( - const std::string& light_name, const ignition::math::Pose3d& pose) + const std::string& light_name, const gz::math::Pose3d& pose) { // Assumes name is unique std::string marker_name = light_name + "_marker"; - ignition::msgs::EntityFactory create_marker_req; + gz::msgs::EntityFactory create_marker_req; create_marker_req.set_sdf(create_light_marker_str(marker_name, pose)); _node.Request("/world/" + _world_name + "/create", create_marker_req, marker_service_cb); @@ -845,9 +845,9 @@ void LightTuning::remove_marker_service(const std::string& light_name) } const std::string& marker_name = it->second.name; - ignition::msgs::Entity remove_marker_req; + gz::msgs::Entity remove_marker_req; remove_marker_req.set_name(marker_name); - remove_marker_req.set_type(ignition::msgs::Entity_Type_MODEL); + remove_marker_req.set_type(gz::msgs::Entity_Type_MODEL); _node.Request("/world/" + _world_name + "/remove", remove_marker_req, marker_service_cb); @@ -954,7 +954,7 @@ void LightTuning::OnSaveLightsBtnPress(const QString& url, } // Register this plugin -IGNITION_ADD_PLUGIN(LightTuning, - ignition::gui::Plugin) +GZ_ADD_PLUGIN(LightTuning, + gz::gui::Plugin) #include "LightTuning.moc" diff --git a/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp b/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp index e5930db..f276d1a 100644 --- a/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp +++ b/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp @@ -18,22 +18,22 @@ #include #include -#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include +#include -#include -#include +#include +#include #include #include @@ -41,13 +41,13 @@ #include #include -using namespace ignition::gazebo; +using namespace gz::sim; using namespace rmf_dispenser_common; using namespace rmf_plugins_utils; namespace rmf_robot_sim_gz_plugins { -class IGNITION_GAZEBO_VISIBLE TeleportDispenserPlugin +class GZ_SIM_VISIBLE TeleportDispenserPlugin : public System, public ISystemConfigure, public ISystemPreUpdate @@ -68,12 +68,11 @@ class IGNITION_GAZEBO_VISIBLE TeleportDispenserPlugin // Stores params representing state of Dispenser, and handles the main dispenser logic std::unique_ptr _dispenser_common; - ignition::transport::Node _ign_node; - ignition::transport::Node::Publisher _item_dispensed_pub; + gz::transport::Node _ign_node; Entity _dispenser; Entity _item_en; // Item that dispenser may contain - ignition::math::AxisAlignedBox _dispenser_vicinity_box; + gz::math::AxisAlignedBox _dispenser_vicinity_box; bool tried_fill_dispenser = false; // Set to true if fill_dispenser() has been called at least once @@ -140,12 +139,12 @@ void TeleportDispenserPlugin::place_on_entity(EntityComponentManager& ecm, // Make service request to Slotcar to get its height instead of accessing // it's AABB component directly - ignition::msgs::Entity req; + gz::msgs::Entity req; req.set_id(base); const unsigned int timeout = 5000; bool result = false; - ignition::msgs::Double rep; + gz::msgs::Double rep; const std::string height_srv_name = "/slotcar_height_" + std::to_string(base); bool executed = _ign_node.Request(height_srv_name, req, timeout, rep, result); @@ -161,18 +160,11 @@ void TeleportDispenserPlugin::place_on_entity(EntityComponentManager& ecm, _dispenser_common->ros_node->get_logger(), "Either base entity or item to be dispensed does not have an AxisAlignedBox component. \ Attempting to dispense item to approximate location."); - new_pose += ignition::math::Pose3(0, 0, 0.5, 0, 0, 0); + new_pose += gz::math::Pose3(0, 0, 0.5, 0, 0, 0); } enableComponent(ecm, to_move); ecm.Component(to_move)->Data() = new_pose; - - // For Ignition slotcar plugin to know when an item has been dispensed to it - // Necessary for TPE Plugin - ignition::msgs::UInt64_V dispense_msg; - dispense_msg.add_data(google::protobuf::uint64(base)); //Conversion from Entity -> uint64 - dispense_msg.add_data(google::protobuf::uint64(to_move)); - _item_dispensed_pub.Publish(dispense_msg); } void TeleportDispenserPlugin::fill_robot_list(EntityComponentManager& ecm, @@ -241,11 +233,11 @@ void TeleportDispenserPlugin::create_dispenser_bounding_box( { const auto dispenser_pos = ecm.Component(_dispenser)->Data().Pos(); - ignition::math::Vector3d corner_1(dispenser_pos.X() - 0.05, + gz::math::Vector3d corner_1(dispenser_pos.X() - 0.05, dispenser_pos.Y() - 0.05, dispenser_pos.Z() - 0.05); - ignition::math::Vector3d corner_2(dispenser_pos.X() + 0.05, + gz::math::Vector3d corner_2(dispenser_pos.X() + 0.05, dispenser_pos.Y() + 0.05, dispenser_pos.Z() + 0.05); - _dispenser_vicinity_box = ignition::math::AxisAlignedBox(corner_1, corner_2); + _dispenser_vicinity_box = gz::math::AxisAlignedBox(corner_1, corner_2); } void TeleportDispenserPlugin::Configure(const Entity& entity, @@ -267,15 +259,6 @@ void TeleportDispenserPlugin::Configure(const Entity& entity, RCLCPP_INFO(_dispenser_common->ros_node->get_logger(), "Started TeleportIngestorPlugin node..."); - // Needed for TPE plugin, so that subscriber knows when to move a payload that - // has been dispensed to it - _item_dispensed_pub = _ign_node.Advertise( - "/item_dispensed"); - if (!_item_dispensed_pub) - { - ignwarn << "Error advertising topic [/item_dispensed]" << std::endl; - } - create_dispenser_bounding_box(ecm); } @@ -323,13 +306,13 @@ void TeleportDispenserPlugin::PreUpdate(const UpdateInfo& info, place_on_entity_cb, check_filled_cb); } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( TeleportDispenserPlugin, System, TeleportDispenserPlugin::ISystemConfigure, TeleportDispenserPlugin::ISystemPreUpdate) // TODO would prefer namespaced -IGNITION_ADD_PLUGIN_ALIAS(TeleportDispenserPlugin, "teleport_dispenser") +GZ_ADD_PLUGIN_ALIAS(TeleportDispenserPlugin, "teleport_dispenser") } // namespace rmf_robot_sim_gz_plugins diff --git a/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp b/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp index 1d36b98..293cf24 100644 --- a/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp +++ b/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp @@ -20,33 +20,33 @@ #include #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #include #include #include -using namespace ignition::gazebo; +using namespace gz::sim; using namespace rmf_ingestor_common; using namespace rmf_plugins_utils; namespace rmf_robot_sim_gz_plugins { -class IGNITION_GAZEBO_VISIBLE TeleportIngestorPlugin +class GZ_SIM_VISIBLE TeleportIngestorPlugin : public System, public ISystemConfigure, public ISystemPreUpdate @@ -72,8 +72,7 @@ class IGNITION_GAZEBO_VISIBLE TeleportIngestorPlugin Entity _ingested_entity; // Item that ingestor may contain rclcpp::Node::SharedPtr _ros_node; - ignition::transport::Node _ign_node; - ignition::transport::Node::Publisher _item_ingested_pub; + gz::transport::Node _gz_node; SimEntity find_nearest_model(const EntityComponentManager& ecm, const std::vector& robot_model_entities, @@ -195,12 +194,6 @@ void TeleportIngestorPlugin::transport_model(EntityComponentManager& ecm) ecm.Component(_ingested_entity)->Data() = { 0, 0, 0}; } - - // For Ignition slotcar plugin to know when an item has been ingested from it - // Necessary for TPE Plugin - ignition::msgs::Entity ingest_msg; - ingest_msg.set_id(_ingested_entity); // Implicit conversion to protobuf::uint64 - _item_ingested_pub.Publish(ingest_msg); } void TeleportIngestorPlugin::send_ingested_item_home( @@ -222,10 +215,10 @@ void TeleportIngestorPlugin::send_ingested_item_home( if (!cmd) { ecm.CreateComponent(_ingested_entity, - components::WorldPoseCmd(ignition::math::Pose3())); + components::WorldPoseCmd(gz::math::Pose3())); } ecm.Component(_ingested_entity)->Data() = - convert_to_pose(it->second); + convert_to_pose(it->second); } _ingestor_common->ingestor_filled = false; } @@ -279,15 +272,6 @@ void TeleportIngestorPlugin::Configure(const Entity& entity, _ingestor_common->init_ros_node(_ros_node); RCLCPP_INFO(_ingestor_common->ros_node->get_logger(), "Started TeleportIngestorPlugin node..."); - - // Needed for TPE plugin, so that subscriber knows when to stop moving a payload that - // has been ingested from it - _item_ingested_pub = _ign_node.Advertise( - "/item_ingested"); - if (!_item_ingested_pub) - { - ignwarn << "Error advertising topic [/item_ingested]" << std::endl; - } } void TeleportIngestorPlugin::PreUpdate(const UpdateInfo& info, @@ -336,13 +320,13 @@ void TeleportIngestorPlugin::PreUpdate(const UpdateInfo& info, get_payload_model_cb, transport_model_cb, send_ingested_item_home_cb); } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( TeleportIngestorPlugin, System, TeleportIngestorPlugin::ISystemConfigure, TeleportIngestorPlugin::ISystemPreUpdate) // TODO would prefer namespaced -IGNITION_ADD_PLUGIN_ALIAS(TeleportIngestorPlugin, "teleport_ingestor") +GZ_ADD_PLUGIN_ALIAS(TeleportIngestorPlugin, "teleport_ingestor") } // namespace rmf_robot_sim_gz_plugins diff --git a/rmf_robot_sim_gz_plugins/src/readonly.cpp b/rmf_robot_sim_gz_plugins/src/readonly.cpp index f90b1a4..75384c9 100644 --- a/rmf_robot_sim_gz_plugins/src/readonly.cpp +++ b/rmf_robot_sim_gz_plugins/src/readonly.cpp @@ -14,12 +14,12 @@ * limitations under the License. * */ -#include +#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -36,11 +36,11 @@ #include #include -using namespace ignition::gazebo; +using namespace gz::sim; namespace rmf_robot_sim_gz_plugins { -class IGNITION_GAZEBO_VISIBLE ReadonlyPlugin +class GZ_SIM_VISIBLE ReadonlyPlugin : public System, public ISystemConfigure, public ISystemPreUpdate @@ -93,13 +93,13 @@ void ReadonlyPlugin::PreUpdate(const UpdateInfo& info, _readonly_common->on_update(pose, sim_time); } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( ReadonlyPlugin, System, ReadonlyPlugin::ISystemConfigure, ReadonlyPlugin::ISystemPreUpdate) // TODO would prefer namespaced -IGNITION_ADD_PLUGIN_ALIAS(ReadonlyPlugin, "readonly") +GZ_ADD_PLUGIN_ALIAS(ReadonlyPlugin, "readonly") } // namespace rmf_robot_sim_gz_plugins diff --git a/rmf_robot_sim_gz_plugins/src/slotcar.cpp b/rmf_robot_sim_gz_plugins/src/slotcar.cpp index 5eac250..d4fb269 100644 --- a/rmf_robot_sim_gz_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_gz_plugins/src/slotcar.cpp @@ -1,36 +1,35 @@ #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include #include #include #include -#include +#include +#include -using namespace ignition::gazebo; +#include -enum class PhysEnginePlugin {DEFAULT, TPE}; -std::unordered_map plugin_names { - {"ignition-physics-tpe-plugin", PhysEnginePlugin::TPE}}; +using namespace gz::sim; -class IGNITION_GAZEBO_VISIBLE SlotcarPlugin +class GZ_SIM_VISIBLE SlotcarPlugin : public System, public ISystemConfigure, public ISystemPreUpdate @@ -46,17 +45,13 @@ class IGNITION_GAZEBO_VISIBLE SlotcarPlugin private: std::unique_ptr dataPtr; - ignition::transport::Node _ign_node; + gz::transport::Node _gz_node; rclcpp::Node::SharedPtr _ros_node; Entity _entity; - std::unordered_set _payloads; std::unordered_set _obstacle_exclusions; double _height = 0; - PhysEnginePlugin phys_plugin = PhysEnginePlugin::DEFAULT; - - bool first_iteration = true; // Flag for checking if it is first PreUpdate() call bool _read_aabb_dimensions = true; bool _remove_world_pose_cmd = false; @@ -64,20 +59,17 @@ class IGNITION_GAZEBO_VISIBLE SlotcarPlugin double _prev_v_command = 0.0; double _prev_w_command = 0.0; - void charge_state_cb(const ignition::msgs::Selection& msg); + void charge_state_cb(const gz::msgs::Selection& msg); void send_control_signals(EntityComponentManager& ecm, const std::pair& displacements, - const std::unordered_set payloads, const double dt, const double target_linear_speed_now, const double target_linear_speed_destination, const std::optional& max_linear_velocity); void init_obstacle_exclusions(EntityComponentManager& ecm); - void item_dispensed_cb(const ignition::msgs::UInt64_V& msg); - void item_ingested_cb(const ignition::msgs::Entity& msg); - bool get_slotcar_height(const ignition::msgs::Entity& req, - ignition::msgs::Double& rep); + bool get_slotcar_height(const gz::msgs::Entity& req, + gz::msgs::Double& rep); std::vector get_obstacle_positions( EntityComponentManager& ecm); @@ -86,14 +78,14 @@ class IGNITION_GAZEBO_VISIBLE SlotcarPlugin void draw_lookahead_marker(); - ignition::msgs::Marker_V _trajectory_marker_msg; + gz::msgs::Marker_V _trajectory_marker_msg; }; SlotcarPlugin::SlotcarPlugin() : dataPtr(std::make_unique()) { // Listen for messages that enable/disable charging - if (!_ign_node.Subscribe("/charge_state", &SlotcarPlugin::charge_state_cb, + if (!_gz_node.Subscribe("/charge_state", &SlotcarPlugin::charge_state_cb, this)) { std::cerr << "Error subscribing to topic [/charge_state]" << std::endl; @@ -136,22 +128,10 @@ void SlotcarPlugin::Configure(const Entity& entity, enableComponent(ecm, entity); enableComponent(ecm, entity); - // Keep track of when a payload is dispensed onto/ingested from slotcar - // Needed for TPE Plugin to know when to manually move payload via this plugin - if (!_ign_node.Subscribe("/item_dispensed", &SlotcarPlugin::item_dispensed_cb, - this)) - { - std::cerr << "Error subscribing to topic [/item_dispensed]" << std::endl; - } - if (!_ign_node.Subscribe("/item_ingested", &SlotcarPlugin::item_ingested_cb, - this)) - { - std::cerr << "Error subscribing to topic [/item_ingested]" << std::endl; - } // Respond to requests asking for height (e.g. for dispenser to dispense object) const std::string height_srv_name = "/slotcar_height_" + std::to_string(entity); - if (!_ign_node.Advertise(height_srv_name, &SlotcarPlugin::get_slotcar_height, + if (!_gz_node.Advertise(height_srv_name, &SlotcarPlugin::get_slotcar_height, this)) { std::cerr << "Error subscribing to topic [/slotcar_height]" << std::endl; @@ -167,7 +147,6 @@ void SlotcarPlugin::Configure(const Entity& entity, void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm, const std::pair& displacements, - const std::unordered_set payloads, const double dt, const double target_linear_speed_now, const double target_linear_speed_destination, @@ -192,20 +171,6 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm, // Update previous velocities _prev_v_command = target_vels[0]; _prev_w_command = target_vels[1]; - - if (phys_plugin == PhysEnginePlugin::TPE) // Need to manually move any payloads - { - for (const Entity& payload : payloads) - { - enableComponent(ecm, payload); - enableComponent(ecm, payload); - - ecm.Component(payload)->Data() = - lin_vel_cmd->Data(); - ecm.Component(payload)->Data() = - ang_vel_cmd->Data(); - } - } } void SlotcarPlugin::init_obstacle_exclusions(EntityComponentManager& ecm) @@ -227,10 +192,12 @@ void SlotcarPlugin::init_obstacle_exclusions(EntityComponentManager& ecm) { c = ::tolower(c); }); - if (n.find("door") != std::string::npos || - n.find("lift") != std::string::npos || + if (ecm.Component(entity) != nullptr || + ecm.Component(entity) != nullptr || n.find("dispensable") != std::string::npos) + { _obstacle_exclusions.insert(entity); + } } return true; }); @@ -266,35 +233,13 @@ std::vector SlotcarPlugin::get_obstacle_positions( return obstacle_positions; } -void SlotcarPlugin::charge_state_cb(const ignition::msgs::Selection& msg) +void SlotcarPlugin::charge_state_cb(const gz::msgs::Selection& msg) { dataPtr->charge_state_cb(msg.name(), msg.selected()); } -// First element of msg should be the slotcar, and the second should be the payload -void SlotcarPlugin::item_dispensed_cb(const ignition::msgs::UInt64_V& msg) -{ - if (msg.data_size() == 2 && msg.data(0) == _entity) - { - Entity new_payload = msg.data(1); - this->_payloads.insert(new_payload); - } -} - -void SlotcarPlugin::item_ingested_cb(const ignition::msgs::Entity& msg) -{ - if (msg.IsInitialized()) - { - const std::unordered_set::iterator it = _payloads.find(msg.id()); - if (it != _payloads.end()) - { - _payloads.erase(it); - } - } -} - -bool SlotcarPlugin::get_slotcar_height(const ignition::msgs::Entity& req, - ignition::msgs::Double& rep) +bool SlotcarPlugin::get_slotcar_height(const gz::msgs::Entity& req, + gz::msgs::Double& rep) { if (req.id() == _entity) { @@ -307,39 +252,39 @@ bool SlotcarPlugin::get_slotcar_height(const ignition::msgs::Entity& req, void SlotcarPlugin::path_request_marker_update( const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg) { - ignition::msgs::Boolean res; + gz::msgs::Boolean res; bool result; for (int i = 0; i < _trajectory_marker_msg.marker_size(); ++i) { auto marker = _trajectory_marker_msg.mutable_marker(i); - marker->set_action(ignition::msgs::Marker::DELETE_ALL); + marker->set_action(gz::msgs::Marker::DELETE_ALL); } - _ign_node.Request( + _gz_node.Request( "/marker_array", _trajectory_marker_msg, 5000, res, result); _trajectory_marker_msg.clear_marker(); auto line_marker = _trajectory_marker_msg.add_marker(); line_marker->set_ns(dataPtr->model_name() + "_line"); line_marker->set_id(1); - line_marker->set_action(ignition::msgs::Marker::ADD_MODIFY); - line_marker->set_type(ignition::msgs::Marker::LINE_STRIP); - line_marker->set_visibility(ignition::msgs::Marker::GUI); - ignition::msgs::Set( + line_marker->set_action(gz::msgs::Marker::ADD_MODIFY); + line_marker->set_type(gz::msgs::Marker::LINE_STRIP); + line_marker->set_visibility(gz::msgs::Marker::GUI); + gz::msgs::Set( line_marker->mutable_material()->mutable_ambient(), - ignition::math::Color(1, 0, 0, 1)); - ignition::msgs::Set( + gz::math::Color(1, 0, 0, 1)); + gz::msgs::Set( line_marker->mutable_material()->mutable_diffuse(), - ignition::math::Color(1, 0, 0, 1)); + gz::math::Color(1, 0, 0, 1)); auto marker_headings = _trajectory_marker_msg.add_marker(); marker_headings->set_id(1); marker_headings->set_ns(dataPtr->model_name() + "_waypoint_headings"); - marker_headings->set_action(ignition::msgs::Marker::ADD_MODIFY); - marker_headings->set_type(ignition::msgs::Marker::LINE_LIST); - marker_headings->set_visibility(ignition::msgs::Marker::GUI); - ignition::msgs::Set(marker_headings->mutable_material()->mutable_ambient(), - ignition::math::Color(0, 1, 0, 1)); - ignition::msgs::Set(marker_headings->mutable_material()->mutable_diffuse(), - ignition::math::Color(0, 1, 0, 1)); + marker_headings->set_action(gz::msgs::Marker::ADD_MODIFY); + marker_headings->set_type(gz::msgs::Marker::LINE_LIST); + marker_headings->set_visibility(gz::msgs::Marker::GUI); + gz::msgs::Set(marker_headings->mutable_material()->mutable_ambient(), + gz::math::Color(0, 1, 0, 1)); + gz::msgs::Set(marker_headings->mutable_material()->mutable_diffuse(), + gz::math::Color(0, 1, 0, 1)); auto& locations = msg->path; double elevation = 0.5; @@ -348,40 +293,40 @@ void SlotcarPlugin::path_request_marker_update( auto loc = locations[i]; // Add points to the trajectory line, slightly elevated for visibility. - ignition::msgs::Set(line_marker->add_point(), - ignition::math::Vector3d(loc.x, loc.y, elevation) + gz::msgs::Set(line_marker->add_point(), + gz::math::Vector3d(loc.x, loc.y, elevation) ); // Draw waypoints - ignition::math::Color waypoint_color(0.0, 1.0, 0.0, 1.0); + gz::math::Color waypoint_color(0.0, 1.0, 0.0, 1.0); auto waypoint_marker = _trajectory_marker_msg.add_marker(); waypoint_marker->set_ns(dataPtr->model_name() + "_waypoints"); waypoint_marker->set_id(i+1); - waypoint_marker->set_action(ignition::msgs::Marker::ADD_MODIFY); - waypoint_marker->set_type(ignition::msgs::Marker::SPHERE); - waypoint_marker->set_visibility(ignition::msgs::Marker::GUI); - ignition::msgs::Set(waypoint_marker->mutable_scale(), - ignition::math::Vector3d(1.5, 1.5, 1.5)); - ignition::msgs::Set( + waypoint_marker->set_action(gz::msgs::Marker::ADD_MODIFY); + waypoint_marker->set_type(gz::msgs::Marker::SPHERE); + waypoint_marker->set_visibility(gz::msgs::Marker::GUI); + gz::msgs::Set(waypoint_marker->mutable_scale(), + gz::math::Vector3d(1.5, 1.5, 1.5)); + gz::msgs::Set( waypoint_marker->mutable_material()->mutable_ambient(), waypoint_color); - ignition::msgs::Set( + gz::msgs::Set( waypoint_marker->mutable_material()->mutable_diffuse(), waypoint_color); - ignition::msgs::Set(waypoint_marker->mutable_pose(), - ignition::math::Pose3d(loc.x, loc.y, elevation, 0, 0, 0)); + gz::msgs::Set(waypoint_marker->mutable_pose(), + gz::math::Pose3d(loc.x, loc.y, elevation, 0, 0, 0)); Eigen::Vector2d dir(cos(loc.yaw), sin(loc.yaw)); double length = 2.0; - ignition::msgs::Set(marker_headings->add_point(), - ignition::math::Vector3d(loc.x, loc.y, elevation)); - ignition::msgs::Set(marker_headings->add_point(), - ignition::math::Vector3d(loc.x + length * dir.x(), + gz::msgs::Set(marker_headings->add_point(), + gz::math::Vector3d(loc.x, loc.y, elevation)); + gz::msgs::Set(marker_headings->add_point(), + gz::math::Vector3d(loc.x + length * dir.x(), loc.y + length * dir.y(), elevation+0.5)); } - _ign_node.Request( + _gz_node.Request( "/marker_array", _trajectory_marker_msg, 5000, res, result); } @@ -390,54 +335,32 @@ void SlotcarPlugin::draw_lookahead_marker() auto lookahead_point = dataPtr->get_lookahead_point(); // Lookahead point - ignition::msgs::Marker marker_msg; + gz::msgs::Marker marker_msg; marker_msg.set_ns(dataPtr->model_name() + "_lookahead_point"); marker_msg.set_id(1); - marker_msg.set_action(ignition::msgs::Marker::ADD_MODIFY); - marker_msg.set_type(ignition::msgs::Marker::CYLINDER); - marker_msg.set_visibility(ignition::msgs::Marker::GUI); + marker_msg.set_action(gz::msgs::Marker::ADD_MODIFY); + marker_msg.set_type(gz::msgs::Marker::CYLINDER); + marker_msg.set_visibility(gz::msgs::Marker::GUI); - ignition::msgs::Set(marker_msg.mutable_pose(), - ignition::math::Pose3d( + gz::msgs::Set(marker_msg.mutable_pose(), + gz::math::Pose3d( lookahead_point(0), lookahead_point(1), lookahead_point(2), 0, 0, 0)); const double scale = 1.5; - ignition::msgs::Set(marker_msg.mutable_scale(), - ignition::math::Vector3d(scale, scale, scale)); - ignition::msgs::Set(marker_msg.mutable_material()->mutable_ambient(), - ignition::math::Color(0, 0, 1, 1)); - ignition::msgs::Set(marker_msg.mutable_material()->mutable_diffuse(), - ignition::math::Color(0, 0, 1, 1)); - _ign_node.Request("/marker", marker_msg); + gz::msgs::Set(marker_msg.mutable_scale(), + gz::math::Vector3d(scale, scale, scale)); + gz::msgs::Set(marker_msg.mutable_material()->mutable_ambient(), + gz::math::Color(0, 0, 1, 1)); + gz::msgs::Set(marker_msg.mutable_material()->mutable_diffuse(), + gz::math::Color(0, 0, 1, 1)); + _gz_node.Request("/marker", marker_msg); } void SlotcarPlugin::PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) { - // Read from components that may not have been initialized in configure() - if (first_iteration) - { - Entity parent = _entity; - while (ecm.ParentEntity(parent)) - { - parent = ecm.ParentEntity(parent); - } - if (ecm.EntityHasComponentType(parent, - components::PhysicsEnginePlugin().TypeId())) - { - const std::string physics_plugin_name = - ecm.Component(parent)->Data(); - const auto it = plugin_names.find(physics_plugin_name); - if (it != plugin_names.end()) - { - phys_plugin = it->second; - } - } - first_iteration = false; - } - // Optimization: Read and store slotcar's dimensions whenever available, then // delete the AABB component once read. Not deleting it causes rtf to drop by // a 3-4x factor whenever the slotcar moves. @@ -455,6 +378,10 @@ void SlotcarPlugin::PreUpdate(const UpdateInfo& info, _read_aabb_dimensions = false; } } + else + { + enableComponent(ecm, _entity); + } } // TODO parallel thread executor? @@ -483,7 +410,7 @@ void SlotcarPlugin::PreUpdate(const UpdateInfo& info, dataPtr->update(rmf_plugins_utils::convert_pose(pose), obstacle_positions, time); - send_control_signals(ecm, {update_result.v, update_result.w}, _payloads, dt, + send_control_signals(ecm, {update_result.v, update_result.w}, dt, update_result.target_linear_speed_now, update_result.target_linear_speed_destination, update_result.max_speed); @@ -495,10 +422,10 @@ void SlotcarPlugin::PreUpdate(const UpdateInfo& info, } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( SlotcarPlugin, System, SlotcarPlugin::ISystemConfigure, SlotcarPlugin::ISystemPreUpdate) -IGNITION_ADD_PLUGIN_ALIAS(SlotcarPlugin, "slotcar") +GZ_ADD_PLUGIN_ALIAS(SlotcarPlugin, "slotcar")