diff --git a/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp b/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp index 7ca99cf..301df52 100644 --- a/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp +++ b/rmf_building_sim_gz_plugins/src/crowd_simulator.cpp @@ -542,7 +542,7 @@ void CrowdSimulatorPlugin::_update_internal_object( 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; + agent_pose = initial_pose * agent_pose; // get components to be updated auto traj_pose_comp = 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 075b321..eea10c6 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 @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include using namespace gz; using namespace gui; @@ -109,4 +109,4 @@ GZ_ADD_PLUGIN(toggle_charging, gz::gui::Plugin) -#include "toggle_charging.moc" \ No newline at end of file +#include "toggle_charging.moc" 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 b0070a9..fbf1a11 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 @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include diff --git a/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp index 1a78e66..6ace407 100644 --- a/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp +++ b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp @@ -15,13 +15,14 @@ * */ +#include "LightTuning.hpp" + #include #include #include #include #include -#include #include #include #include @@ -34,9 +35,9 @@ #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, @@ -81,7 +82,7 @@ std::optional parse_light_type(const std::string& type) { return sdf::LightType::SPOT; } - ignwarn << "Unable to parse \"" << type << + gzwarn << "Unable to parse \"" << type << "\" as a light type. Using previous value." << std::endl; return std::nullopt; } @@ -97,7 +98,7 @@ std::optional parse_pose(const std::string& pose_str) } else { - ignwarn << "Unable to parse \"" << pose_str << + gzwarn << "Unable to parse \"" << pose_str << "\" as a pose. Using previous value." << std::endl; return std::nullopt; } @@ -114,7 +115,7 @@ std::optional parse_color(const std::string& color_str) } else { - ignwarn << "Unable to parse \"" << color_str << + gzwarn << "Unable to parse \"" << color_str << "\" as a color. Using previous value." << std::endl; return std::nullopt; } @@ -131,7 +132,7 @@ std::optional parse_double(const std::string& double_str) } else { - ignwarn << "Unable to parse \"" << double_str << + gzwarn << "Unable to parse \"" << double_str << "\" as a double. Using previous value." << std::endl; return std::nullopt; } @@ -149,7 +150,7 @@ std::optional parse_vector( } else { - ignwarn << "Unable to parse \"" << vector_str << + gzwarn << "Unable to parse \"" << vector_str << "\" as a vector. Using previous value." << std::endl; return std::nullopt; } @@ -167,7 +168,7 @@ std::optional parse_angle( } else { - ignwarn << "Unable to parse \"" << angle_str << + gzwarn << "Unable to parse \"" << angle_str << "\" as an angle in radians. Using previous value." << std::endl; return std::nullopt; } @@ -209,67 +210,6 @@ std::ostream& operator<<(std::ostream& os, const sdf::Light& light) return os; } -// Data model representing a list of lights. Provides data for delegates to display -// in QML via a series of roles which the delegates bind to. -class LightsModel : public QAbstractListModel -{ - Q_OBJECT - Q_ENUMS(Roles) - -public: - enum Roles - { - NameRole = Qt::UserRole + 1, - PoseRole, - IndexRole, - DiffuseRole, - SpecularRole, - AttenuationRangeRole, - AttenuationConstantRole, - AttenuationLinearRole, - AttenuationQuadraticRole, - DirectionRole, - SpotInnerAngleRole, - SpotOuterAngleRole, - SpotFalloffRole - }; - - QHash roleNames() const override; - int rowCount(const QModelIndex& parent = QModelIndex()) const override; - QVariant data(const QModelIndex& index, - int role = Qt::DisplayRole) const override; - // Note: We do not need to override and define the setData() function since - // we only update our model from the GUI in `LightTuning::OnCreateLight` - - // Inserts a default light with the name specified in `name_qstr`, - // if it does not exist - void add_new_light(const QString& name_qstr); - // Deletes the light from LightsModel at index `idx` if it exists - void remove_light(int idx); - - // Returns a reference to the light at index `idx` in `_lights`. - // Assumes `idx` is valid. - sdf::Light& get_light(int idx); - // Returns a reference to the light with name `name` in `_lights`. - // Assumes `name` is a valid name belonging to some light in `_lights`. - sdf::Light& get_light(const std::string& name); - // Returns a const reference to a QVector of all lights in the LightsModel - // object - const QVector& get_lights() const; - - // Fill _existing_names with names from the ecm - void populate_names(gz::sim::EntityComponentManager& ecm); - -private: - // Collection of lights, each with a unique name (enforced by - // add_new_light) - QVector _lights; - - // Set of existing names in the simulation. Used to ensure no name collisions - // when creating new lights - std::unordered_set _pre_existing_names; -}; - QHash LightsModel::roleNames() const { return {{ NameRole, "name"}, @@ -374,7 +314,7 @@ void LightsModel::add_new_light(const QString& name_qstr) if (pre_existing_it != _pre_existing_names.end() || lights_it != _lights.end() || name.size() < 1) { - ignerr << "Light names must be unique and at least 1 character long." << + gzerr << "Light names must be unique and at least 1 character long." << std::endl; return; } @@ -390,7 +330,7 @@ void LightsModel::remove_light(int idx) { if (idx >= _lights.size()) { - ignerr << "Light to remove does not exist." << std::endl; + gzerr << "Light to remove does not exist." << std::endl; return; } @@ -432,94 +372,6 @@ void LightsModel::populate_names(gz::sim::EntityComponentManager& ecm) }); } -// Class that handles all GUI interactions and their associated -// light creations/deletions -class LightTuning : public gz::sim::GuiSystem -{ - Q_OBJECT - -public: - virtual void LoadConfig(const tinyxml2::XMLElement* _pluginElem) - override; - - void Update(const gz::sim::UpdateInfo& _info, - gz::sim::EntityComponentManager& _ecm) override; - -signals: - void poseChanged(QString nm, QString new_pose); - void markerSelected(QString nm); - -public slots: - void OnLightTypeSelect(sdf::Light& light, const QString& type); - void OnCreateLightBtnPress( - int idx, bool cast_shadow, const QString& type, - const QString& name, const QString& pose_str, - const QString& diffuse_str, const QString& specular_str, - const QString& attentuation_range_str, - const QString& attentuation_constant_str, - const QString& attentuation_linear_str, - const QString& attentuation_quadratic_str, - const QString& direction_str, - const QString& spot_inner_angle_str, - const QString& spot_outer_angle_str, - const QString& spot_falloff_str); - void OnRemoveLightBtnPress(int idx, const QString& name); - void OnAddLightFormBtnPress(const QString& name); - void OnSaveLightsBtnPress(const QString& url, bool save_all = true, - int idx = -1); - -protected: bool eventFilter(QObject* _obj, QEvent* _event) override; - -private: - bool first_update_call = true; - const std::string sdf_open_tag = " \n"; - const std::string sdf_close_tag = ""; - - std::string _world_name; - gz::transport::Node _node; - LightsModel _model; - 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 - // light pose - struct LightMarker - { - std::string name; // Name of the LightMarker - 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; - // Map from light name to its corresponding marker - std::unordered_map _markers; - - enum class Action {REMOVE, CREATE}; - // Map from a light name to the latest update/creation/removal request - // for that corresponding light - std::unordered_map actions; - - // Returns a string representation of the specified light in the - // SDF v1.7 format - std::string light_to_sdf_string(const sdf::Light&); - - // 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 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); - - // Gets and stores a pointer to scene from the Rendering singleton - void load_scene(); - // Handles light creation/update/deletion requests stored in `actions` - void render_lights(); - // Displays the light in `_model` that has the name `light_name` - void create_light_rendering(const std::string& light_name); - // Removes from the scene the light with name `light_name` - void remove_light_rendering(const std::string& light_name); -}; void LightTuning::LoadConfig(const tinyxml2::XMLElement*) { @@ -634,14 +486,14 @@ void LightTuning::load_scene() const std::string& engineName = loadedEngNames[0]; if (loadedEngNames.size() > 1) { - igndbg << "More than one engine is available. " << + gzdbg << "More than one engine is available. " << "Grid config plugin will use engine [" << engineName << "]" << std::endl; } auto engine = gz::rendering::engine(engineName); if (!engine) { - ignerr << "Internal error: failed to load engine [" << engineName << + gzerr << "Internal error: failed to load engine [" << engineName << "]. Grid plugin won't work." << std::endl; return; } @@ -658,7 +510,7 @@ void LightTuning::render_lights() if (!scene_ptr || !scene_ptr->IsInitialized() || nullptr == scene_ptr->RootVisual()) { - ignerr << "Internal error: scene is null." << std::endl; + gzerr << "Internal error: scene is null." << std::endl; return; } @@ -754,7 +606,7 @@ void LightTuning::create_light_rendering(const std::string& name) if (!light_ptr) { - ignerr << "Unable to create or update light with name " << + gzerr << "Unable to create or update light with name " << name << std::endl; return; } @@ -839,7 +691,7 @@ void LightTuning::remove_marker_service(const std::string& light_name) _markers.find(light_name); if (it == _markers.end()) { - ignwarn << "Unable to remove any marker belonging to light with name " << + gzwarn << "Unable to remove any marker belonging to light with name " << light_name << std::endl; return; } @@ -927,7 +779,7 @@ void LightTuning::OnSaveLightsBtnPress(const QString& url, std::ofstream file(path); if (!file) { - ignerr << "Unable to open file for writing." << std::endl; + gzerr << "Unable to open file for writing." << std::endl; return; } @@ -947,14 +799,12 @@ void LightTuning::OnSaveLightsBtnPress(const QString& url, } else { - ignerr << "Invalid index given. No light saved to file." << std::endl; + gzerr << "Invalid index given. No light saved to file." << std::endl; } file.close(); - ignmsg << "File saved to: " << path << std::endl; + gzmsg << "File saved to: " << path << std::endl; } // Register this plugin GZ_ADD_PLUGIN(LightTuning, gz::gui::Plugin) - -#include "LightTuning.moc" diff --git a/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.hpp b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.hpp new file mode 100644 index 0000000..19afa81 --- /dev/null +++ b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.hpp @@ -0,0 +1,173 @@ +/* + * 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 + +// Data model representing a list of lights. Provides data for delegates to display +// in QML via a series of roles which the delegates bind to. +class LightsModel : public QAbstractListModel +{ + Q_OBJECT + Q_ENUMS(Roles) + +public: + enum Roles + { + NameRole = Qt::UserRole + 1, + PoseRole, + IndexRole, + DiffuseRole, + SpecularRole, + AttenuationRangeRole, + AttenuationConstantRole, + AttenuationLinearRole, + AttenuationQuadraticRole, + DirectionRole, + SpotInnerAngleRole, + SpotOuterAngleRole, + SpotFalloffRole + }; + + QHash roleNames() const override; + int rowCount(const QModelIndex& parent = QModelIndex()) const override; + QVariant data(const QModelIndex& index, + int role = Qt::DisplayRole) const override; + // Note: We do not need to override and define the setData() function since + // we only update our model from the GUI in `LightTuning::OnCreateLight` + + // Inserts a default light with the name specified in `name_qstr`, + // if it does not exist + void add_new_light(const QString& name_qstr); + // Deletes the light from LightsModel at index `idx` if it exists + void remove_light(int idx); + + // Returns a reference to the light at index `idx` in `_lights`. + // Assumes `idx` is valid. + sdf::Light& get_light(int idx); + // Returns a reference to the light with name `name` in `_lights`. + // Assumes `name` is a valid name belonging to some light in `_lights`. + sdf::Light& get_light(const std::string& name); + // Returns a const reference to a QVector of all lights in the LightsModel + // object + const QVector& get_lights() const; + + // Fill _existing_names with names from the ecm + void populate_names(gz::sim::EntityComponentManager& ecm); + +private: + // Collection of lights, each with a unique name (enforced by + // add_new_light) + QVector _lights; + + // Set of existing names in the simulation. Used to ensure no name collisions + // when creating new lights + std::unordered_set _pre_existing_names; +}; +// Class that handles all GUI interactions and their associated +// light creations/deletions +class LightTuning : public gz::sim::GuiSystem +{ + Q_OBJECT + +public: + virtual void LoadConfig(const tinyxml2::XMLElement* _pluginElem) + override; + + void Update(const gz::sim::UpdateInfo& _info, + gz::sim::EntityComponentManager& _ecm) override; + +signals: + void poseChanged(QString nm, QString new_pose); + void markerSelected(QString nm); + +public slots: + void OnLightTypeSelect(sdf::Light& light, const QString& type); + void OnCreateLightBtnPress( + int idx, bool cast_shadow, const QString& type, + const QString& name, const QString& pose_str, + const QString& diffuse_str, const QString& specular_str, + const QString& attentuation_range_str, + const QString& attentuation_constant_str, + const QString& attentuation_linear_str, + const QString& attentuation_quadratic_str, + const QString& direction_str, + const QString& spot_inner_angle_str, + const QString& spot_outer_angle_str, + const QString& spot_falloff_str); + void OnRemoveLightBtnPress(int idx, const QString& name); + void OnAddLightFormBtnPress(const QString& name); + void OnSaveLightsBtnPress(const QString& url, bool save_all = true, + int idx = -1); + +protected: bool eventFilter(QObject* _obj, QEvent* _event) override; + +private: + bool first_update_call = true; + const std::string sdf_open_tag = " \n"; + const std::string sdf_close_tag = ""; + + std::string _world_name; + gz::transport::Node _node; + LightsModel _model; + 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 + // light pose + struct LightMarker + { + std::string name; // Name of the LightMarker + 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; + // Map from light name to its corresponding marker + std::unordered_map _markers; + + enum class Action {REMOVE, CREATE}; + // Map from a light name to the latest update/creation/removal request + // for that corresponding light + std::unordered_map actions; + + // Returns a string representation of the specified light in the + // SDF v1.7 format + std::string light_to_sdf_string(const sdf::Light&); + + // 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 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); + + // Gets and stores a pointer to scene from the Rendering singleton + void load_scene(); + // Handles light creation/update/deletion requests stored in `actions` + void render_lights(); + // Displays the light in `_model` that has the name `light_name` + void create_light_rendering(const std::string& light_name); + // Removes from the scene the light with name `light_name` + void remove_light_rendering(const std::string& light_name); +}; diff --git a/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp b/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp index 4dd81fa..2810449 100644 --- a/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp +++ b/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp @@ -160,7 +160,7 @@ 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 += gz::math::Pose3(0, 0, 0.5, 0, 0, 0); + new_pose = gz::math::Pose3(0, 0, 0.5, 0, 0, 0) * new_pose; } enableComponent(ecm, to_move); @@ -251,7 +251,7 @@ void TeleportDispenserPlugin::Configure(const Entity& entity, _dispenser = entity; _dispenser_common->guid = ecm.Component(_dispenser)->Data(); - ignwarn << "Initializing plugin with name " << _dispenser_common->guid << + gzwarn << "Initializing plugin with name " << _dispenser_common->guid << std::endl; _ros_node = std::make_shared(_dispenser_common->guid); diff --git a/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp b/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp index ac1c297..ac94009 100644 --- a/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp +++ b/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp @@ -267,8 +267,8 @@ void TeleportIngestorPlugin::Configure(const Entity& entity, _ingestor = entity; _ingestor_common->_guid = ecm.Component(_ingestor)->Data(); - ignwarn << "Initializing plugin with name " << _ingestor_common->_guid - << std::endl; + gzwarn << "Initializing plugin with name " << _ingestor_common->_guid + << std::endl; _ros_node = std::make_shared(_ingestor_common->_guid); _ingestor_common->init_ros_node(_ros_node); RCLCPP_INFO(_ingestor_common->ros_node->get_logger(),