diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0206d99944..a415239685 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -8,7 +8,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - uses: actions/setup-python@v3 - uses: pre-commit/action@v3.0.0 with: @@ -25,7 +25,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - uses: actions/setup-python@v3 - uses: pre-commit/action@v3.0.0 with: diff --git a/COPYING b/COPYING deleted file mode 100644 index d9a10c0d8e..0000000000 --- a/COPYING +++ /dev/null @@ -1,176 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS diff --git a/LICENSE b/LICENSE index 69fc1e1045..d9a10c0d8e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,15 +1,176 @@ -Software License Agreement (Apache License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright 2018 Open Source Robotics Foundation + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -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 + 1. Definitions. - http://www.apache.org/licenses/LICENSE-2.0 + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. -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. + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/include/gz/sim/Actor.hh b/include/gz/sim/Actor.hh new file mode 100644 index 0000000000..2cace92808 --- /dev/null +++ b/include/gz/sim/Actor.hh @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2023 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 GZ_SIM_ACTOR_HH_ +#define GZ_SIM_ACTOR_HH_ + +#include +#include +#include + +#include + +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Actor Actor.hh gz/sim/Actor.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a actor + /// entity. + /// + /// For example, given an actor's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Actor actor(entity); + /// std::string name = actor.Name(ecm); + /// + class GZ_SIM_VISIBLE Actor + { + /// \brief Constructor + /// \param[in] _entity Actor entity + public: explicit Actor(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Actor is related to. + /// \return Actor entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New actor entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this actor correctly refers to an entity that + /// has a components::Actor. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid actor in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the actor's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Actor's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the actor. + /// If the actor has a trajectory, this will only return the origin + /// pose of the trajectory and not the actual world pose of the actor. + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the actor or nullopt if the entity does not + /// have a components::Pose component. + /// \sa WorldPose + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the trajectory pose of the actor. There are two + /// ways that the actor can follow a trajectory: 1) SDF script, + /// 2) manually setting trajectory pose. This function retrieves 2) the + /// manual trajectory pose set by the user. The Trajectory pose is + /// given relative to the trajectory pose origin returned by Pose(). + /// \param[in] _ecm Entity Component manager. + /// \return Trajectory pose of the actor w.r.t. to trajectory origin. + /// \sa Pose + public: std::optional TrajectoryPose( + const EntityComponentManager &_ecm) const; + + /// \brief Set the trajectory pose of the actor. There are two + /// ways that the actor can follow a trajectory: 1) SDF script, + /// 2) manually setting trajectory pose. This function enables option 2). + /// Manually setting the trajectory pose will override the scripted + /// trajectory specified in SDF. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _name Trajectory pose w.r.t. to the trajectory origin + /// \sa Pose + public: void SetTrajectoryPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose); + + /// \brief Get the world pose of the actor. + /// This returns the current world pose of the actor computed by gazebo. + /// The world pose is the combination of the actor's pose and its + /// trajectory pose. The currently trajectory pose is either manually set + /// via SetTrajectoryPose or interpolated from waypoints in the SDF script + /// based on the current time. + /// \param[in] _ecm Entity-component manager. + /// \return World pose of the actor or nullopt if the entity does not + /// have a components::WorldPose component. + /// \sa Pose + public: std::optional WorldPose( + const EntityComponentManager &_ecm) const; + + /// \brief Set the name of animation to use for this actor. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _name Animation name + public: void SetAnimationName(EntityComponentManager &_ecm, + const std::string &_name); + + /// \brief Set the time of animation for this actor. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _time Animation time + public: void SetAnimationTime(EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_time); + + /// \brief Get the name of animation used by the actor + /// \param[in] _ecm Entity-component manager. + /// \return Animation name + public: std::optional AnimationName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the time of animation for this actor + /// \param[in] _ecm Entity-component manager. + /// \return Animation time + public: std::optional AnimationTime( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sim/Joint.hh b/include/gz/sim/Joint.hh new file mode 100644 index 0000000000..8de460c544 --- /dev/null +++ b/include/gz/sim/Joint.hh @@ -0,0 +1,284 @@ +/* + * Copyright (C) 2023 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 GZ_SIM_JOINT_HH_ +#define GZ_SIM_JOINT_HH_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Joint Joint.hh gz/sim/Joint.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a joint + /// entity. + /// + /// For example, given a joint's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Joint joint(entity); + /// std::string name = joint.Name(ecm); + /// + class GZ_SIM_VISIBLE Joint + { + /// \brief Constructor + /// \param[in] _entity Joint entity + public: explicit Joint(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Joint is related to. + /// \return Joint entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New joint entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this joint correctly refers to an entity that + /// has a components::Joint. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid joint in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the joint's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Joint's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the parent link name + /// \param[in] _ecm Entity-component manager. + /// \return Parent link name or nullopt if the entity does not have a + /// components::ParentLinkName component. + public: std::optional ParentLinkName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the child link name + /// \param[in] _ecm Entity-component manager. + /// \return Child link name or nullopt if the entity does not have a + /// components::ChildLinkName component. + public: std::optional ChildLinkName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the joint or nullopt if the entity does not + /// have a components::Pose component. + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the thread pitch of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Thread pitch of the joint or nullopt if the entity does not + /// have a components::ThreadPitch component. + public: std::optional ThreadPitch( + const EntityComponentManager &_ecm) const; + + /// \brief Get the joint axis + /// \param[in] _ecm Entity-component manager. + /// \return Axis of the joint or nullopt if the entity does not + /// have a components::JointAxis or components::JointAxis2 component. + public: std::optional> Axis( + const EntityComponentManager &_ecm) const; + + /// \brief Get the joint type + /// \param[in] _ecm Entity-component manager. + /// \return Type of the joint or nullopt if the entity does not + /// have a components::JointType component. + public: std::optional Type( + const EntityComponentManager &_ecm) const; + + /// \brief Get the ID of a sensor entity which is an immediate child of + /// this joint. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Sensor name. + /// \return Sensor entity. + public: sim::Entity SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + + /// \brief Get all sensors which are immediate children of this joint. + /// \param[in] _ecm Entity-component manager. + /// \return All sensors in this joint. + public: std::vector Sensors( + const EntityComponentManager &_ecm) const; + + /// \brief Get the number of sensors which are immediate children of this + /// joint. + /// \param[in] _ecm Entity-component manager. + /// \return Number of sensors in this joint. + public: uint64_t SensorCount(const EntityComponentManager &_ecm) const; + + /// \brief Set velocity on this joint. Only applied if no forces are set + /// \param[in] _ecm Entity Component manager. + /// \param[in] _velocities Joint velocities commands (target velocities). + /// This is different from ResetVelocity in that this does not modify the + /// internal state of the joint. Instead, the physics engine is expected + /// to compute the necessary joint torque for the commanded velocity and + /// apply it in the next simulation step. The vector of velocities should + /// have the same size as the degrees of freedom of the joint. + public: void SetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities); + + /// \brief Set force on this joint. If both forces and velocities are set, + /// only forces are applied + /// \param[in] _ecm Entity Component manager. + /// \param[in] _forces Joint force or torque commands (target forces + /// or toruqes). The vector of forces should have the same size as the + /// degrees of freedom of the joint. + public: void SetForce(EntityComponentManager &_ecm, + const std::vector &_forces); + + /// \brief Set the velocity limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum velocity limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetVelocityLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Set the effort limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum effort limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetEffortLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Set the position limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum position limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetPositionLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Reset the joint positions + /// \param[in] _ecm Entity Component manager. + /// \param[in] _positions Joint positions to reset to. + /// The vector of positions should have the same size as the degrees of + /// freedom of the joint. + public: void ResetPosition(EntityComponentManager &_ecm, + const std::vector &_positions); + + /// \brief Reset the joint velocities + /// \param[in] _ecm Entity Component manager. + /// \param[in] _velocities Joint velocities to reset to. This is different + /// from SetVelocity as this modifies the internal state of the joint. + /// The vector of velocities should have the same size as the degrees of + /// freedom of the joint. + public: void ResetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities); + + /// \brief By default, Gazebo will not report velocities for a joint, so + /// functions like `Velocity` will return nullopt. This + /// function can be used to enable joint velocity check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableVelocityCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief By default, Gazebo will not report positions for a joint, so + /// functions like `Position` will return nullopt. This + /// function can be used to enable joint position check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnablePositionCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief By default, Gazebo will not report transmitted wrench for a + /// joint, so functions like `TransmittedWrench` will return nullopt. This + /// function can be used to enable joint transmitted wrench check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableTransmittedWrenchCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief Get the velocity of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Velocity of the joint or nullopt if velocity check + /// is not enabled. + /// \sa EnableVelocityCheck + public: std::optional> Velocity( + const EntityComponentManager &_ecm) const; + + /// \brief Get the position of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Position of the joint or nullopt if position check + /// is not enabled. + /// \sa EnablePositionCheck + public: std::optional> Position( + const EntityComponentManager &_ecm) const; + + /// \brief Get the position of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Transmitted wrench of the joint or nullopt if transmitted + /// wrench check is not enabled. + /// \sa EnableTransmittedWrenchCheck + public: std::optional> TransmittedWrench( + const EntityComponentManager &_ecm) const; + + /// \brief Get the parent model + /// \param[in] _ecm Entity-component manager. + /// \return Parent Model or nullopt if the entity does not have a + /// components::ParentEntity component. + public: std::optional ParentModel( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sim/Light.hh b/include/gz/sim/Light.hh new file mode 100644 index 0000000000..13cad9c3be --- /dev/null +++ b/include/gz/sim/Light.hh @@ -0,0 +1,291 @@ +/* + * Copyright (C) 2023 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 GZ_SIM_LIGHT_HH_ +#define GZ_SIM_LIGHT_HH_ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Light Light.hh gz/sim/Light.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a light + /// entity. + /// + /// For example, given a light's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Light light(entity); + /// std::string name = light.Name(ecm); + /// + class GZ_SIM_VISIBLE Light + { + /// \brief Constructor + /// \param[in] _entity Light entity + public: explicit Light(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Light is related to. + /// \return Light entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New light entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this light correctly refers to an entity that + /// has a components::Light. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid light in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the light's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Light's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the light. + /// The pose is given w.r.t the light's parent. which can be a world or + /// a link. + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the light or nullopt if the entity does not + /// have a components::Pose component. + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light type + /// \param[in] _ecm Entity-component manager. + /// \return Type of the light or nullopt if the entity does not + /// have a components::LightType component. + public: std::optional Type( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light diffuse color + /// \param[in] _ecm Entity-component manager. + /// \return Diffuse color of the light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional DiffuseColor( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light specular color + /// \param[in] _ecm Entity-component manager. + /// \return Specular color of the light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional SpecularColor( + const EntityComponentManager &_ecm) const; + + /// \brief Get whether the light casts shadows + /// \param[in] _ecm Entity-component manager. + /// \return Cast shadow bool value of light or nullopt if the entity does + /// not have a components::CastShadows component. + public: std::optional CastShadows( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light intensity. + /// \param[in] _ecm Entity-component manager. + /// \return Intensity of light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional Intensity( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light direction. + /// \param[in] _ecm Entity-component manager. + /// \return Direction of light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional Direction( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation range. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation range of light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional AttenuationRange( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation constant value. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation constant value of light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional AttenuationConstant( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation linear value. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation linear value of light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional AttenuationLinear( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation quadratic value. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation quadratic value of light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional AttenuationQuadratic( + const EntityComponentManager &_ecm) const; + + /// \brief Get the inner angle of light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \return Inner angle of spot light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional SpotInnerAngle( + const EntityComponentManager &_ecm) const; + + /// \brief Get the outer angle of light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \return Outer angle of spot light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional SpotOuterAngle( + const EntityComponentManager &_ecm) const; + + /// \brief Get the fall off value of light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \return Fall off value of spot light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional SpotFalloff( + const EntityComponentManager &_ecm) const; + + /// \brief Set the pose of this light. + /// \param[in] _ecm Entity-component manager. + /// Pose is set w.r.t. the light's parent which can be a world or a link. + /// \param[in] _pose Pose to set the light to. + public: void SetPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose); + + /// \brief Set the diffuse color of this light. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _color Diffuse color to set the light to + public: void SetDiffuseColor(EntityComponentManager &_ecm, + const math::Color &_color); + + /// \brief Set the specular color of this light. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _color Specular color to set the light to + public: void SetSpecularColor(EntityComponentManager &_ecm, + const math::Color &_color); + + /// \brief Set whether the light casts shadows. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _bool True to cast shadows, false to not cast shadows. + public: void SetCastShadows(EntityComponentManager &_ecm, + bool _castShadows); + + /// \brief Set light intensity. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _value Intensity value + public: void SetIntensity(EntityComponentManager &_ecm, + double _value); + + /// \brief Set light direction. Applies to directional lights + /// \param[in] _ecm Entity-component manager. + /// and spot lights only. + /// \param[in] _dir Direction to set + public: void SetDirection(EntityComponentManager &_ecm, + const math::Vector3d &_dir); + + /// \brief Set attenuation range of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _bool True to cast shadows, false to not cast shadows. + public: void SetAttenuationRange(EntityComponentManager &_ecm, + double _range); + + /// \brief Set attenuation constant value of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _value Attenuation constant value to set + public: void SetAttenuationConstant(EntityComponentManager &_ecm, + double _value); + + /// \brief Set attenuation linear value of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _value Attenuation linear value to set + public: void SetAttenuationLinear(EntityComponentManager &_ecm, + double _value); + + /// \brief Set attenuation quadratic value of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _value Attenuation quadratic value to set + public: void SetAttenuationQuadratic(EntityComponentManager &_ecm, + double _value); + + /// \brief Set inner angle for this light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _angle Angle to set. + public: void SetSpotInnerAngle(EntityComponentManager &_ecm, + const math::Angle &_angle); + + /// \brief Set outer angle for this light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _angle Angle to set. + public: void SetSpotOuterAngle(EntityComponentManager &_ecm, + const math::Angle &_angle); + + /// \brief Set fall off value for this light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _fallOff Fall off value + public: void SetSpotFalloff(EntityComponentManager &_ecm, + double _falloff); + + /// \brief Get the parent entity. This can be a world or a link. + /// \param[in] _ecm Entity-component manager. + /// \return Parent entity or nullopt if the entity does not have a + /// components::ParentEntity component. + public: std::optional Parent( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sim/Sensor.hh b/include/gz/sim/Sensor.hh new file mode 100644 index 0000000000..e2558bee99 --- /dev/null +++ b/include/gz/sim/Sensor.hh @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2023 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 GZ_SIM_SENSOR_HH_ +#define GZ_SIM_SENSOR_HH_ + +#include +#include +#include + +#include + +#include + +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Sensor Sensor.hh gz/sim/Sensor.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a sensor + /// entity. + /// + /// For example, given a sensor's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Sensor sensor(entity); + /// std::string name = sensor.Name(ecm); + /// + class GZ_SIM_VISIBLE Sensor + { + /// \brief Constructor + /// \param[in] _entity Sensor entity + public: explicit Sensor(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Sensor is related to. + /// \return Sensor entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New sensor entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this sensor correctly refers to an entity that + /// has a components::Sensor. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid sensor in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the sensor's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Sensor's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the sensor + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the sensor or nullopt if the entity does not + /// have a components::Pose component. + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the topic of the sensor + /// \param[in] _ecm Entity-component manager. + /// \return Topic of the sensor or nullopt if the entity does not + /// have a components::SensorTopic component. + public: std::optional Topic( + const EntityComponentManager &_ecm) const; + + /// \brief Get the parent entity. This can be a link or a joint. + /// \param[in] _ecm Entity-component manager. + /// \return Parent entity or nullopt if the entity does not have a + /// components::ParentEntity component. + public: std::optional Parent( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/ignition/gazebo/Actor.hh b/include/ignition/gazebo/Actor.hh new file mode 100644 index 0000000000..a8bd091180 --- /dev/null +++ b/include/ignition/gazebo/Actor.hh @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2023 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 diff --git a/include/ignition/gazebo/Joint.hh b/include/ignition/gazebo/Joint.hh new file mode 100644 index 0000000000..8b4269d202 --- /dev/null +++ b/include/ignition/gazebo/Joint.hh @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2023 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 diff --git a/include/ignition/gazebo/Light.hh b/include/ignition/gazebo/Light.hh new file mode 100644 index 0000000000..087d73bc21 --- /dev/null +++ b/include/ignition/gazebo/Light.hh @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2023 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 diff --git a/include/ignition/gazebo/Sensor.hh b/include/ignition/gazebo/Sensor.hh new file mode 100644 index 0000000000..e6325f0464 --- /dev/null +++ b/include/ignition/gazebo/Sensor.hh @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2023 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 diff --git a/src/Actor.cc b/src/Actor.cc new file mode 100644 index 0000000000..a949ac00b6 --- /dev/null +++ b/src/Actor.cc @@ -0,0 +1,169 @@ +/* + * Copyright (C) 2023 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 "gz/sim/components/Actor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" + +#include "gz/sim/Actor.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + +class gz::sim::Actor::Implementation +{ + /// \brief Id of actor entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Actor::Actor(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Actor::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Actor::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Actor::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Actor::Name(const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Actor::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Actor::WorldPose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +void Actor::SetTrajectoryPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose) +{ + auto pose = + _ecm.Component(this->dataPtr->id); + + if (!pose) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::TrajectoryPose(_pose)); + } + else + { + pose->Data() = _pose; + } +} + +////////////////////////////////////////////////// +std::optional Actor::TrajectoryPose( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +void Actor::SetAnimationName(EntityComponentManager &_ecm, + const std::string &_name) +{ + auto animName = + _ecm.Component(this->dataPtr->id); + + if (!animName) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::AnimationName(_name)); + } + else + { + animName->Data() = _name; + } +} + +////////////////////////////////////////////////// +void Actor::SetAnimationTime(EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_time) +{ + auto animTime = + _ecm.Component(this->dataPtr->id); + + if (!animTime) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::AnimationTime(_time)); + } + else + { + animTime->Data() = _time; + } +} + +////////////////////////////////////////////////// +std::optional Actor::AnimationName( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Actor::AnimationTime( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} diff --git a/src/Actor_TEST.cc b/src/Actor_TEST.cc new file mode 100644 index 0000000000..92f50e26df --- /dev/null +++ b/src/Actor_TEST.cc @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2023 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 "gz/sim/Actor.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/components/Actor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" + +///////////////////////////////////////////////// +TEST(ActorTest, Constructor) +{ + gz::sim::Actor actorNull; + EXPECT_EQ(gz::sim::kNullEntity, actorNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + EXPECT_EQ(id, actor.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Actor actorCopy(actor); // NOLINT + EXPECT_EQ(actor.Entity(), actorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + gz::sim::Actor actorCopy; + actorCopy = actor; + EXPECT_EQ(actor.Entity(), actorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + gz::sim::Actor actorMoved(std::move(actor)); + EXPECT_EQ(id, actorMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + gz::sim::Actor actorMoved; + actorMoved = std::move(actor); + EXPECT_EQ(id, actorMoved.Entity()); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6f26f06366..01f49907d2 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -47,18 +47,22 @@ set(cli_sources ) set (sources + Actor.cc Barrier.cc BaseView.cc Conversions.cc ComponentFactory.cc EntityComponentManager.cc EntityComponentManagerDiff.cc + Joint.cc LevelManager.cc + Light.cc Link.cc Model.cc Primitives.cc SdfEntityCreator.cc SdfGenerator.cc + Sensor.cc Server.cc ServerConfig.cc ServerPrivate.cc @@ -77,6 +81,7 @@ set (sources set (gtest_sources ${gtest_sources} + Actor_TEST.cc AddedMass_TEST.cc Barrier_TEST.cc BaseView_TEST.cc @@ -85,11 +90,14 @@ set (gtest_sources Conversions_TEST.cc EntityComponentManager_TEST.cc EventManager_TEST.cc + Joint_TEST.cc + Light_TEST.cc Link_TEST.cc Model_TEST.cc Primitives_TEST.cc SdfEntityCreator_TEST.cc SdfGenerator_TEST.cc + Sensor_TEST.cc ServerConfig_TEST.cc Server_TEST.cc SimulationRunner_TEST.cc diff --git a/src/Joint.cc b/src/Joint.cc new file mode 100644 index 0000000000..8147897e4e --- /dev/null +++ b/src/Joint.cc @@ -0,0 +1,386 @@ +/* + * Copyright (C) 2023 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 "gz/sim/components/ChildLinkName.hh" +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/JointAxis.hh" +#include "gz/sim/components/JointEffortLimitsCmd.hh" +#include "gz/sim/components/JointForceCmd.hh" +#include "gz/sim/components/JointPosition.hh" +#include "gz/sim/components/JointPositionLimitsCmd.hh" +#include "gz/sim/components/JointPositionReset.hh" +#include "gz/sim/components/JointTransmittedWrench.hh" +#include "gz/sim/components/JointType.hh" +#include "gz/sim/components/JointVelocity.hh" +#include "gz/sim/components/JointVelocityCmd.hh" +#include "gz/sim/components/JointVelocityLimitsCmd.hh" +#include "gz/sim/components/JointVelocityReset.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/ParentLinkName.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/ThreadPitch.hh" + +#include "gz/sim/Joint.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + + +class gz::sim::Joint::Implementation +{ + /// \brief Id of joint entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Joint::Joint(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Joint::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Joint::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Joint::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Joint::Name(const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Joint::ParentLinkName( + const EntityComponentManager &_ecm) const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::ChildLinkName( + const EntityComponentManager &_ecm) const +{ + auto child = _ecm.Component(this->dataPtr->id); + + if (!child) + return std::nullopt; + + return std::optional(child->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::ThreadPitch( + const EntityComponentManager &_ecm) const +{ + auto threadPitch = _ecm.Component(this->dataPtr->id); + + if (!threadPitch) + return std::nullopt; + + return std::optional(threadPitch->Data()); +} + +////////////////////////////////////////////////// +std::optional> Joint::Axis( + const EntityComponentManager &_ecm) const +{ + auto axis = _ecm.Component(this->dataPtr->id); + if (!axis) + return std::nullopt; + + std::vector axisVec{axis->Data()}; + + auto axis2 = _ecm.Component(this->dataPtr->id); + if (axis2) + axisVec.push_back(axis2->Data()); + + return std::optional>(axisVec); +} + +////////////////////////////////////////////////// +std::optional Joint::Type( + const EntityComponentManager &_ecm) const +{ + auto jointType = _ecm.Component(this->dataPtr->id); + + if (!jointType) + return std::nullopt; + + return std::optional(jointType->Data()); +} + +////////////////////////////////////////////////// +Entity Joint::SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Sensor()); +} + +////////////////////////////////////////////////// +std::vector Joint::Sensors(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Sensor()); +} + +////////////////////////////////////////////////// +uint64_t Joint::SensorCount(const EntityComponentManager &_ecm) const +{ + return this->Sensors(_ecm).size(); +} + +////////////////////////////////////////////////// +void Joint::SetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities) +{ + auto jointVelocityCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityCmd(_velocities)); + } + else + { + jointVelocityCmd->Data() = _velocities; + } +} + +////////////////////////////////////////////////// +void Joint::SetForce(EntityComponentManager &_ecm, + const std::vector &_forces) +{ + auto jointForceCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointForceCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointForceCmd(_forces)); + } + else + { + jointForceCmd->Data() = _forces; + } +} + +////////////////////////////////////////////////// +void Joint::SetVelocityLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointVelocityLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityLimitsCmd(_limits)); + } + else + { + jointVelocityLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::SetEffortLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointEffortLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointEffortLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointEffortLimitsCmd(_limits)); + } + else + { + jointEffortLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::SetPositionLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointPosLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointPosLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointPositionLimitsCmd(_limits)); + } + else + { + jointPosLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::ResetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities) +{ + auto jointVelocityReset = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityReset) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityReset(_velocities)); + } + else + { + jointVelocityReset->Data() = _velocities; + } +} + +////////////////////////////////////////////////// +void Joint::ResetPosition(EntityComponentManager &_ecm, + const std::vector &_positions) +{ + auto jointPositionReset = + _ecm.Component(this->dataPtr->id); + + if (!jointPositionReset) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointPositionReset(_positions)); + } + else + { + jointPositionReset->Data() = _positions; + } +} + +////////////////////////////////////////////////// +void Joint::EnableVelocityCheck(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +void Joint::EnablePositionCheck(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +void Joint::EnableTransmittedWrenchCheck(EntityComponentManager &_ecm, + bool _enable) const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +std::optional> Joint::Velocity( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional> Joint::Position( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional> Joint::TransmittedWrench( + const EntityComponentManager &_ecm) const +{ + // TransmittedWrench components contains one wrench msg value + // instead of a vector like JointPosition and JointVelocity + // components. + // todo(anyone) change JointTransmittedWrench to store a vector + // of wrench msgs? + // We provide an API that returns a vector which is consistent + // with Velocity and Position accessor functions + auto comp = _ecm.Component( + this->dataPtr->id); + if (!comp) + return std::nullopt; + std::vector wrenchVec{comp->Data()}; + return wrenchVec; +} + +////////////////////////////////////////////////// +std::optional Joint::ParentModel(const EntityComponentManager &_ecm) + const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc new file mode 100644 index 0000000000..9994684aa9 --- /dev/null +++ b/src/Joint_TEST.cc @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2023 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 "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Joint.hh" +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" + +///////////////////////////////////////////////// +TEST(JointTest, Constructor) +{ + gz::sim::Joint jointNull; + EXPECT_EQ(gz::sim::kNullEntity, jointNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + EXPECT_EQ(id, joint.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Joint jointCopy(joint); // NOLINT + EXPECT_EQ(joint.Entity(), jointCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + gz::sim::Joint jointCopy; + jointCopy = joint; + EXPECT_EQ(joint.Entity(), jointCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + gz::sim::Joint jointMoved(std::move(joint)); + EXPECT_EQ(id, jointMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + gz::sim::Joint jointMoved; + jointMoved = std::move(joint); + EXPECT_EQ(id, jointMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, Sensors) +{ + // jointA + // - sensorAA + // - sensorAB + // + // jointC + + gz::sim::EntityComponentManager ecm; + + // Joint A + auto jointAEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointAEntity, gz::sim::components::Joint()); + ecm.CreateComponent(jointAEntity, + gz::sim::components::Name("jointA_name")); + + // Sensor AA - Child of Joint A + auto sensorAAEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorAAEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::Name("sensorAA_name")); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::ParentEntity(jointAEntity)); + + // Sensor AB - Child of Joint A + auto sensorABEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorABEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::Name("sensorAB_name")); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::ParentEntity(jointAEntity)); + + // Joint C + auto jointCEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointCEntity, gz::sim::components::Joint()); + ecm.CreateComponent(jointCEntity, + gz::sim::components::Name("jointC_name")); + + std::size_t foundSensors = 0; + + gz::sim::Joint jointA(jointAEntity); + auto sensors = jointA.Sensors(ecm); + EXPECT_EQ(2u, sensors.size()); + for (const auto &sensor : sensors) + { + if (sensor == sensorAAEntity || sensor == sensorABEntity) + foundSensors++; + } + EXPECT_EQ(foundSensors, sensors.size()); + + gz::sim::Joint jointC(jointCEntity); + EXPECT_EQ(0u, jointC.Sensors(ecm).size()); +} diff --git a/src/Light.cc b/src/Light.cc new file mode 100644 index 0000000000..bcec0ca0b2 --- /dev/null +++ b/src/Light.cc @@ -0,0 +1,522 @@ +/* + * Copyright (C) 2023 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 "gz/sim/components/Light.hh" +#include "gz/sim/components/LightType.hh" +#include "gz/sim/components/LightCmd.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" + +#include "gz/sim/Light.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + + +class gz::sim::Light::Implementation +{ + /// \brief Id of light entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Light::Light(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Light::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Light::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Light::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Light::Name(const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Light::Type( + const EntityComponentManager &_ecm) const +{ + auto lightType = _ecm.Component(this->dataPtr->id); + + if (!lightType) + return std::nullopt; + + return std::optional(lightType->Data()); +} + +////////////////////////////////////////////////// +std::optional Light::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Light::CastShadows( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().CastShadows()); +} + +////////////////////////////////////////////////// +std::optional Light::Intensity( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Intensity()); +} + +////////////////////////////////////////////////// +std::optional Light::Direction( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Direction()); +} + +////////////////////////////////////////////////// +std::optional Light::DiffuseColor( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Diffuse()); +} + +////////////////////////////////////////////////// +std::optional Light::SpecularColor( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Specular()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationRange( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().AttenuationRange()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationConstant( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().ConstantAttenuationFactor()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationLinear( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().LinearAttenuationFactor()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationQuadratic( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().QuadraticAttenuationFactor()); +} + +////////////////////////////////////////////////// +std::optional Light::SpotInnerAngle( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().SpotInnerAngle()); +} + +////////////////////////////////////////////////// +std::optional Light::SpotOuterAngle( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().SpotOuterAngle()); +} + +////////////////////////////////////////////////// +std::optional Light::SpotFalloff( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().SpotFalloff()); +} + +////////////////////////////////////////////////// +void Light::SetPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_pose(), _pose); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetCastShadows(EntityComponentManager &_ecm, + bool _castShadows) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_cast_shadows(_castShadows); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetIntensity(EntityComponentManager &_ecm, + double _intensity) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_intensity(_intensity); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetDirection(EntityComponentManager &_ecm, + const math::Vector3d &_dir) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_direction(), _dir); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetDiffuseColor(EntityComponentManager &_ecm, + const math::Color &_color) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_diffuse(), _color); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpecularColor(EntityComponentManager &_ecm, + const math::Color &_color) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_specular(), _color); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetAttenuationRange(EntityComponentManager &_ecm, + double _range) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_range(_range); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetAttenuationConstant(EntityComponentManager &_ecm, + double _value) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_attenuation_constant(_value); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} +////////////////////////////////////////////////// +void Light::SetAttenuationLinear(EntityComponentManager &_ecm, + double _value) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_attenuation_linear(_value); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetAttenuationQuadratic(EntityComponentManager &_ecm, + double _value) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_attenuation_quadratic(_value); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpotInnerAngle(EntityComponentManager &_ecm, + const math::Angle &_angle) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_spot_inner_angle(_angle.Radian()); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpotOuterAngle(EntityComponentManager &_ecm, + const math::Angle &_angle) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_spot_outer_angle(_angle.Radian()); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpotFalloff(EntityComponentManager &_ecm, + double _falloff) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_spot_falloff(_falloff); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +std::optional Light::Parent(const EntityComponentManager &_ecm) const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} diff --git a/src/Light_TEST.cc b/src/Light_TEST.cc new file mode 100644 index 0000000000..0d3023367c --- /dev/null +++ b/src/Light_TEST.cc @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2023 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 "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Light.hh" +#include "gz/sim/components/Light.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" + +///////////////////////////////////////////////// +TEST(LightTest, Constructor) +{ + gz::sim::Light lightNull; + EXPECT_EQ(gz::sim::kNullEntity, lightNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Light light(id); + + EXPECT_EQ(id, light.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Light lightCopy(light); // NOLINT + EXPECT_EQ(light.Entity(), lightCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + gz::sim::Light lightCopy; + lightCopy = light; + EXPECT_EQ(light.Entity(), lightCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + gz::sim::Light lightMoved(std::move(light)); + EXPECT_EQ(id, lightMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + gz::sim::Light lightMoved; + lightMoved = std::move(light); + EXPECT_EQ(id, lightMoved.Entity()); +} diff --git a/src/Sensor.cc b/src/Sensor.cc new file mode 100644 index 0000000000..e9c9939f07 --- /dev/null +++ b/src/Sensor.cc @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2023 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 "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" + +#include "gz/sim/Sensor.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + + +class gz::sim::Sensor::Implementation +{ + /// \brief Id of sensor entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Sensor::Sensor(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Sensor::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Sensor::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Sensor::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Sensor::Name(const EntityComponentManager &_ecm) + const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Sensor::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Sensor::Topic( + const EntityComponentManager &_ecm) const +{ + auto topic = _ecm.Component(this->dataPtr->id); + + if (!topic) + return std::nullopt; + + return std::optional(topic->Data()); +} + +////////////////////////////////////////////////// +std::optional Sensor::Parent(const EntityComponentManager &_ecm) const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc new file mode 100644 index 0000000000..5c07367a0e --- /dev/null +++ b/src/Sensor_TEST.cc @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2023 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 "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Sensor.hh" + +///////////////////////////////////////////////// +TEST(SensorTest, Constructor) +{ + gz::sim::Sensor sensorNull; + EXPECT_EQ(gz::sim::kNullEntity, sensorNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + EXPECT_EQ(id, sensor.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Sensor sensorCopy(sensor); // NOLINT + EXPECT_EQ(sensor.Entity(), sensorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + gz::sim::Sensor sensorCopy; + sensorCopy = sensor; + EXPECT_EQ(sensor.Entity(), sensorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + gz::sim::Sensor sensorMoved(std::move(sensor)); + EXPECT_EQ(id, sensorMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + gz::sim::Sensor sensorMoved; + sensorMoved = std::move(sensor); + EXPECT_EQ(id, sensorMoved.Entity()); +} diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index d18af02b68..cc451cf264 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -79,6 +80,13 @@ namespace gz::sim /// \brief Distance of the camera to the model public: double distanceMoveToModel = 0.0; + /// \brief Camera horizontal fov + public: double horizontalFov = 0.0; + + /// \brief Flag indicating if there is a new camera horizontalFOV + /// coming from qml side + public: bool newHorizontalFOV = false; + /// \brief gui camera pose public: math::Pose3d camPose; @@ -102,6 +110,11 @@ namespace gz::sim /// \return True if there is a new view controller from gui camera public: bool UpdateQtViewControl(); + /// \brief Checks if there is new camera horizontal fov from gui camera, + /// used to update qml side + /// \return True if there is a new horizontal fov from gui camera + public: bool UpdateQtCamHorizontalFOV(); + /// \brief User camera public: rendering::CameraPtr camera{nullptr}; @@ -174,6 +187,12 @@ bool ViewAngle::eventFilter(QObject *_obj, QEvent *_event) this->CamClipDistChanged(); } + // updates qml cam horizontal fov spin boxes if changed + if (this->dataPtr->UpdateQtCamHorizontalFOV()) + { + this->CamHorizontalFOVChanged(); + } + if (this->dataPtr->UpdateQtViewControl()) { this->ViewControlIndexChanged(); @@ -392,6 +411,19 @@ void ViewAngle::CamPoseCb(const msgs::Pose &_msg) } } +///////////////////////////////////////////////// +double ViewAngle::HorizontalFOV() const +{ + return this->dataPtr->horizontalFov; +} + +///////////////////////////////////////////////// +void ViewAngle::SetHorizontalFOV(double _horizontalFOV) +{ + this->dataPtr->horizontalFov = _horizontalFOV; + this->dataPtr->newHorizontalFOV = true; +} + ///////////////////////////////////////////////// QList ViewAngle::CamClipDist() const { @@ -525,6 +557,13 @@ void ViewAnglePrivate::OnRender() this->camera->SetFarClipPlane(this->camClipDist[1]); this->newCamClipDist = false; } + + // Camera horizontalFOV + if (this->newHorizontalFOV) + { + this->camera->SetHFOV(gz::math::Angle(this->horizontalFov)); + this->newHorizontalFOV = false; + } } ///////////////////////////////////////////////// @@ -563,6 +602,18 @@ void ViewAnglePrivate::OnComplete() } } +///////////////////////////////////////////////// +bool ViewAnglePrivate::UpdateQtCamHorizontalFOV() +{ + bool updated = false; + if (std::abs(this->camera->HFOV().Radian() - this->horizontalFov) > 0.0001) + { + this->horizontalFov = this->camera->HFOV().Radian(); + updated = true; + } + return updated; +} + ///////////////////////////////////////////////// bool ViewAnglePrivate::UpdateQtCamClipDist() { diff --git a/src/gui/plugins/view_angle/ViewAngle.hh b/src/gui/plugins/view_angle/ViewAngle.hh index f316ebb386..134f2e2b6d 100644 --- a/src/gui/plugins/view_angle/ViewAngle.hh +++ b/src/gui/plugins/view_angle/ViewAngle.hh @@ -62,6 +62,13 @@ namespace sim NOTIFY ViewControlIndexChanged ) + /// \brief gui camera horizontal fov + Q_PROPERTY( + double horizontalFOV + READ HorizontalFOV + NOTIFY CamHorizontalFOVChanged + ) + /// \brief Constructor public: ViewAngle(); @@ -97,6 +104,16 @@ namespace sim /// \param[in] _sensitivity View control sensitivity vlaue public slots: void OnViewControlSensitivity(double _sensitivity); + /// \brief Updates gui camera's Horizontal fov + /// \param[in] _horizontalFOV Horizontal fov + public slots: void SetHorizontalFOV(double _horizontalFOV); + + /// \brief Get the current gui horizontal fov. + public: Q_INVOKABLE double HorizontalFOV() const; + + /// \brief Notify that the gui camera's horizontal fov changed + signals: void CamHorizontalFOVChanged(); + /// \brief Get the current gui camera pose. public: Q_INVOKABLE QList CamPose() const; diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index eabc851d95..3750f4e4c5 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -326,6 +326,41 @@ ColumnLayout { } } + // Set camera's horizontal FOV + Text { + text: "Horizontal FOV" + Layout.fillWidth: true + color: Material.Grey + leftPadding: 5 + topPadding: 10 + font.bold: true + } + + GridLayout { + width: parent.width + columns: 2 + + Text { + text: "HorizontalFOV (rads)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: horizontalFOV + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 1 + value: ViewAngle.horizontalFOV + maximumValue: 3.14159 + minimumValue: 0.000001 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetHorizontalFOV(horizontalFOV.value) + } + } + // Bottom spacer Item { width: 10 diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 6ae9968a46..e79767dd5d 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -79,6 +79,7 @@ #include "gz/sim/components/Scene.hh" #include "gz/sim/components/SegmentationCamera.hh" #include "gz/sim/components/SemanticLabel.hh" +#include "gz/sim/components/Sensor.hh" #include "gz/sim/components/SourceFilePath.hh" #include "gz/sim/components/SphericalCoordinates.hh" #include "gz/sim/components/Temperature.hh" @@ -290,6 +291,9 @@ class gz::sim::RenderUtilPrivate public: std::unordered_map newParticleEmittersCmds; + /// \brief New sensor topics that should be added to ECM as new components + public: std::unordered_map newSensorTopics; + /// \brief A list of entities with particle emitter cmds to remove public: std::vector particleCmdsToRemove; @@ -355,6 +359,11 @@ class gz::sim::RenderUtilPrivate /// \brief A map of entity ids and actor animation info. public: std::unordered_map actorAnimationData; + /// \brief A map of entity ids and the world pose of actor at current + /// timestep. The pose data is used to update the WorldPose component in the + /// ECM + public: std::unordered_map actorWorldPoses; + /// \brief True to update skeletons manually using bone poses /// (see actorTransforms). False to let render engine update animation /// based on sim time. @@ -761,6 +770,35 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/, _ecm.RemoveComponent(entity); } } + + // create sensor topic components + { + for (const auto &it : this->dataPtr->newSensorTopics) + { + // Set topic + _ecm.CreateComponent(it.first, components::SensorTopic(it.second)); + } + this->dataPtr->newSensorTopics.clear(); + } + + + // update actor world pose + { + for (const auto &it : this->dataPtr->actorWorldPoses) + { + // set world pose + auto worldPoseComp = _ecm.Component(it.first); + if (!worldPoseComp) + { + _ecm.CreateComponent(it.first, components::WorldPose(it.second)); + } + else + { + worldPoseComp->Data() = it.second; + } + } + this->dataPtr->actorWorldPoses.clear(); + } } ////////////////////////////////////////////////// @@ -1338,7 +1376,13 @@ void RenderUtil::Update() trajPose.Rot() = tf.second["actorPose"].Rotation(); } - actorVisual->SetLocalPose(globalPose * trajPose); + math::Pose3d worldPose = globalPose * trajPose; + actorVisual->SetLocalPose(worldPose); + { + // populate world pose map which is used to update ECM + std::lock_guard lock(this->dataPtr->updateMutex); + this->dataPtr->actorWorldPoses[tf.first] = worldPose; + } tf.second.erase("actorPose"); actorMesh->SetSkeletonLocalTransforms(tf.second); @@ -1643,6 +1687,7 @@ void RenderUtilPrivate::AddNewSensor(const EntityComponentManager &_ecm, { sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); } + this->newSensorTopics[_entity] = sdfDataCopy.Topic(); this->newSensors.push_back( std::make_tuple(_entity, std::move(sdfDataCopy), _parent)); this->sensorEntities.insert(_entity); @@ -3205,7 +3250,12 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_mapSetLocalPose(globalPose * trajPose); + math::Pose3d worldPose = globalPose * trajPose; + actorVisual->SetLocalPose(worldPose); + + // populate world pose map which is used to update ECM + std::lock_guard lock(this->updateMutex); + this->actorWorldPoses[it.first] = worldPose; } } diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 86410f4765..95e560a439 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -1,6 +1,7 @@ set(TEST_TYPE "INTEGRATION") set(tests + actor.cc ackermann_steering_system.cc acoustic_comms.cc air_pressure_system.cc @@ -32,6 +33,7 @@ set(tests hydrodynamics.cc hydrodynamics_flags.cc imu_system.cc + joint.cc joint_controller_system.cc joint_position_controller_system.cc joint_state_publisher_system.cc @@ -40,6 +42,7 @@ set(tests lift_drag_system.cc level_manager.cc level_manager_runtime_performers.cc + light.cc link.cc logical_camera_system.cc logical_audio_sensor_plugin.cc @@ -67,6 +70,7 @@ set(tests scene_broadcaster_system.cc sdf_frame_semantics.cc sdf_include.cc + sensor.cc spherical_coordinates.cc thruster.cc touch_plugin.cc diff --git a/test/integration/actor.cc b/test/integration/actor.cc new file mode 100644 index 0000000000..9540c6a5ca --- /dev/null +++ b/test/integration/actor.cc @@ -0,0 +1,282 @@ +/* + * Copyright (C) 2023 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 "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class ActorIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Actor actor; + EXPECT_FALSE(actor.Valid(ecm)); + } + + // Missing actor component + { + auto id = ecm.CreateEntity(); + Actor actor(id); + EXPECT_FALSE(actor.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + EXPECT_TRUE(actor.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No name + EXPECT_EQ(std::nullopt, actor.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("actor_name")); + EXPECT_EQ("actor_name", actor.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No pose + EXPECT_EQ(std::nullopt, actor.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, actor.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, WorldPose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No pose + EXPECT_EQ(std::nullopt, actor.WorldPose(ecm)); + + // Add world pose + math::Pose3d pose(0, 3, 9, 0.0, 1.2, -3.0); + ecm.CreateComponent(id, + components::WorldPose(pose)); + EXPECT_EQ(pose, actor.WorldPose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, TrajectoryPose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No pose + EXPECT_EQ(std::nullopt, actor.TrajectoryPose(ecm)); + + // Add pose + math::Pose3d pose(3, 42, 35, 2.1, 3.2, 1.3); + ecm.CreateComponent(id, + components::TrajectoryPose(pose)); + EXPECT_EQ(pose, actor.TrajectoryPose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, SetTrajectoryPose) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // No TrajectoryPose should exist by default + EXPECT_EQ(nullptr, ecm.Component(eActor)); + + math::Pose3d pose(0.1, 2.3, 4.7, 0, 0, 1.0); + actor.SetTrajectoryPose(ecm, pose); + + // trajectory pose should exist + EXPECT_NE(nullptr, ecm.Component(eActor)); + EXPECT_EQ(pose, + ecm.Component(eActor)->Data()); + + // Make sure the trajectory pose is updated + math::Pose3d pose2(1.0, 3.2, 7.4, 1.0, 0, 0.0); + actor.SetTrajectoryPose(ecm, pose2); + EXPECT_EQ(pose2, + ecm.Component(eActor)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, SetAnimationName) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // No AnimationName should exist by default + EXPECT_EQ(nullptr, ecm.Component(eActor)); + + std::string animName = "animation_name"; + actor.SetAnimationName(ecm, animName); + + // animation name should exist + EXPECT_NE(nullptr, ecm.Component(eActor)); + EXPECT_EQ(animName, + ecm.Component(eActor)->Data()); + + // Make sure the animation name is updated + std::string animName2 = "animation_name2"; + actor.SetAnimationName(ecm, animName2); + EXPECT_EQ(animName2, + ecm.Component(eActor)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, SetAnimationTime) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // No AnimationTime should exist by default + EXPECT_EQ(nullptr, ecm.Component(eActor)); + + std::chrono::steady_clock::duration animTime = std::chrono::milliseconds(30); + actor.SetAnimationTime(ecm, animTime); + + // animation time should exist + EXPECT_NE(nullptr, ecm.Component(eActor)); + EXPECT_EQ(animTime, + ecm.Component(eActor)->Data()); + + // Make sure the animation time is updated + std::chrono::steady_clock::duration animTime2 = std::chrono::milliseconds(70); + actor.SetAnimationTime(ecm, animTime2); + EXPECT_EQ(animTime2, + ecm.Component(eActor)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, AnimationName) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // animation name should return nullopt by default + EXPECT_EQ(std::nullopt, actor.AnimationName(ecm)); + + // get animation name + std::string animName = "animation_name"; + ecm.SetComponentData(eActor, animName); + EXPECT_EQ(animName, actor.AnimationName(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, AnimationTime) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // animation time should return nullopt by default + EXPECT_EQ(std::nullopt, actor.AnimationTime(ecm)); + + // get animation time + std::chrono::steady_clock::duration animTime = std::chrono::milliseconds(60); + ecm.CreateComponent(eActor, + components::AnimationTime(animTime)); + EXPECT_EQ(animTime, actor.AnimationTime(ecm)); +} diff --git a/test/integration/joint.cc b/test/integration/joint.cc new file mode 100644 index 0000000000..12044025c6 --- /dev/null +++ b/test/integration/joint.cc @@ -0,0 +1,601 @@ +/* + * Copyright (C) 2023 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class JointIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Joint joint; + EXPECT_FALSE(joint.Valid(ecm)); + } + + // Missing joint component + { + auto id = ecm.CreateEntity(); + Joint joint(id); + EXPECT_FALSE(joint.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + EXPECT_TRUE(joint.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No name + EXPECT_EQ(std::nullopt, joint.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("joint_name")); + EXPECT_EQ("joint_name", joint.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, JointNames) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No parent or child joint names + EXPECT_EQ(std::nullopt, joint.ParentLinkName(ecm)); + EXPECT_EQ(std::nullopt, joint.ChildLinkName(ecm)); + + // Add parent joint name + ecm.CreateComponent(id, + components::ParentLinkName("parent_joint_name")); + EXPECT_EQ("parent_joint_name", joint.ParentLinkName(ecm)); + + // Add child joint name + ecm.CreateComponent(id, + components::ChildLinkName("child_joint_name")); + EXPECT_EQ("child_joint_name", joint.ChildLinkName(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No pose + EXPECT_EQ(std::nullopt, joint.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, joint.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Type) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No joint type + EXPECT_EQ(std::nullopt, joint.Type(ecm)); + + // Add type + sdf::JointType jointType = sdf::JointType::PRISMATIC; + ecm.CreateComponent(id, + components::JointType(jointType)); + EXPECT_EQ(jointType, joint.Type(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Axis) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No joint axis + EXPECT_EQ(std::nullopt, joint.Axis(ecm)); + + // Add axis + sdf::JointAxis jointAxis; + auto errors = jointAxis.SetXyz(math::Vector3d(0, 1, 1)); + EXPECT_TRUE(errors.empty()); + ecm.CreateComponent(id, + components::JointAxis(jointAxis)); + EXPECT_EQ(jointAxis.Xyz(), (*joint.Axis(ecm))[0].Xyz()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ThreadPitch) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No name + EXPECT_EQ(std::nullopt, joint.ThreadPitch(ecm)); + + // Add name + ecm.CreateComponent(id, + components::ThreadPitch(1.23)); + EXPECT_DOUBLE_EQ(1.23, *joint.ThreadPitch(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SensorByName) +{ + EntityComponentManager ecm; + + // Joint + auto eJoint = ecm.CreateEntity(); + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + EXPECT_EQ(0u, joint.SensorCount(ecm)); + + // Sensor + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + ecm.CreateComponent(eSensor, + components::ParentEntity(eJoint)); + ecm.CreateComponent(eSensor, + components::Name("sensor_name")); + + // Check joint + EXPECT_EQ(eSensor, joint.SensorByName(ecm, "sensor_name")); + EXPECT_EQ(1u, joint.SensorCount(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetVelocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector velCmd{1}; + joint.SetVelocity(ecm, velCmd); + + // velocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(velCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity cmd is updated + std::vector velCmd2{0}; + joint.SetVelocity(ecm, velCmd2); + EXPECT_EQ(velCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetForce) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointForceCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector forceCmd{10}; + joint.SetForce(ecm, forceCmd); + + // velocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(forceCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the force cmd is updated + std::vector forceCmd2{1}; + joint.SetForce(ecm, forceCmd2); + EXPECT_EQ(forceCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetVelocityLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector velLimitsCmd{math::Vector2d(0.1, 1.1)}; + joint.SetVelocityLimits(ecm, velLimitsCmd); + + // velocity limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(velLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity limits cmd is updated + std::vector velLimitsCmd2{math::Vector2d(-0.2, 2.4)}; + joint.SetVelocityLimits(ecm, velLimitsCmd2); + EXPECT_EQ(velLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetEffortLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointEffortLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector effortLimitsCmd{math::Vector2d(9, 9.9)}; + joint.SetEffortLimits(ecm, effortLimitsCmd); + + // effort limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(effortLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the effort limits cmd is updated + std::vector effortLimitsCmd2{math::Vector2d(5.2, 5.4)}; + joint.SetEffortLimits(ecm, effortLimitsCmd2); + EXPECT_EQ(effortLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetPositionLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointPositionLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector positionLimitsCmd{math::Vector2d(-0.1, 0.1)}; + joint.SetPositionLimits(ecm, positionLimitsCmd); + + // position limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(positionLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the position limits cmd is updated + std::vector positionLimitsCmd2{math::Vector2d(-0.2, 0.4)}; + joint.SetPositionLimits(ecm, positionLimitsCmd2); + EXPECT_EQ(positionLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ResetVelocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityReset should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector vel{1}; + joint.ResetVelocity(ecm, vel); + + // velocity reset should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(vel, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity reset is updated + std::vector vel2{0}; + joint.ResetVelocity(ecm, vel2); + EXPECT_EQ(vel2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ResetPosition) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointPositionReset should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector pos{1}; + joint.ResetPosition(ecm, pos); + + // position reset should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(pos, + ecm.Component(eJoint)->Data()); + + // Make sure the position reset is updated + std::vector pos2{0}; + joint.ResetPosition(ecm, pos2); + EXPECT_EQ(pos2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Velocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, velocity should return nullopt + EXPECT_EQ(std::nullopt, joint.Velocity(ecm)); + + // After enabling, velocity should return default values + joint.EnableVelocityCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector velOut = *joint.Velocity(ecm); + EXPECT_TRUE(velOut.empty()); + + // With custom velocities + std::vector vel{0.3, 0.4}; + ecm.SetComponentData(eJoint, vel); + velOut = *joint.Velocity(ecm); + EXPECT_EQ(2u, velOut.size()); + EXPECT_DOUBLE_EQ(vel[0], velOut[0]); + EXPECT_DOUBLE_EQ(vel[1], velOut[1]); + + // Disabling velocities goes back to nullopt + joint.EnableVelocityCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.Velocity(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Position) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, position should return nullopt + EXPECT_EQ(std::nullopt, joint.Position(ecm)); + + // After enabling, position should return default values + joint.EnablePositionCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector posOut = *joint.Position(ecm); + EXPECT_TRUE(posOut.empty()); + + // With custom positions + std::vector pos{-0.3, -0.4}; + ecm.SetComponentData(eJoint, pos); + posOut = *joint.Position(ecm); + EXPECT_EQ(2u, posOut.size()); + EXPECT_DOUBLE_EQ(pos[0], posOut[0]); + EXPECT_DOUBLE_EQ(pos[1], posOut[1]); + + // Disabling positions goes back to nullopt + joint.EnablePositionCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.Position(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, TransmittedWrench) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, wrench should return nullopt + EXPECT_EQ(std::nullopt, joint.TransmittedWrench(ecm)); + + // After enabling, wrench should return default values + joint.EnableTransmittedWrenchCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector wrenchOut = *joint.TransmittedWrench(ecm); + // todo(anyone) Unlike Velocity and Position functions that return an + // empty vector if it has not been populated yet, the wrench vector + // will contain one empty wrench msg. This is because the TransmittedWrench + // API workarounds the fact that the TransmittedWrench component contains + // only one wrench reading instead of a wrench vector like positions and + // velocities. + // EXPECT_TRUE(wrenchOut.empty()); + + // With custom wrench + msgs::Wrench wrenchMsg; + msgs::Set(wrenchMsg.mutable_force(), + math::Vector3d(0.2, 3.2, 0.1)); + + std::vector wrench{wrenchMsg}; + ecm.SetComponentData(eJoint, wrench[0]); + wrenchOut = *joint.TransmittedWrench(ecm); + EXPECT_EQ(1u, wrenchOut.size()); + EXPECT_DOUBLE_EQ(wrench[0].force().x(), wrenchOut[0].force().x()); + EXPECT_DOUBLE_EQ(wrench[0].force().y(), wrenchOut[0].force().y()); + EXPECT_DOUBLE_EQ(wrench[0].force().z(), wrenchOut[0].force().z()); + + // Disabling wrench goes back to nullopt + joint.EnableTransmittedWrenchCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.TransmittedWrench(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ParentModel) +{ + EntityComponentManager ecm; + + // Model + auto eModel = ecm.CreateEntity(); + ecm.CreateComponent(eModel, components::Model()); + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + EXPECT_FALSE(joint.ParentModel(ecm).has_value()); + + ecm.CreateComponent(eJoint, + components::ParentEntity(eModel)); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Check parent model + EXPECT_EQ(eModel, ecm.ParentEntity(eJoint)); + auto parentModel = joint.ParentModel(ecm); + ASSERT_TRUE(parentModel.has_value()); + EXPECT_TRUE(parentModel->Valid(ecm)); + EXPECT_EQ(eModel, parentModel->Entity()); +} diff --git a/test/integration/light.cc b/test/integration/light.cc new file mode 100644 index 0000000000..292d0b8366 --- /dev/null +++ b/test/integration/light.cc @@ -0,0 +1,804 @@ +/* + * Copyright (C) 2023 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 +#include +#include +#include +#include + +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class LightIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Light light; + EXPECT_FALSE(light.Valid(ecm)); + } + + // Missing light component + { + auto id = ecm.CreateEntity(); + Light light(id); + EXPECT_FALSE(light.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + EXPECT_TRUE(light.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + // No name + EXPECT_EQ(std::nullopt, light.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("light_name")); + EXPECT_EQ("light_name", light.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + // No pose + EXPECT_EQ(std::nullopt, light.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, light.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Type) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + // No light type + EXPECT_EQ(std::nullopt, light.Type(ecm)); + + // Add type + std::string lightType = "point"; + ecm.CreateComponent(id, + components::LightType(lightType)); + EXPECT_EQ(lightType, light.Type(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, CastShadows) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + lightSdf.SetCastShadows(true); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_TRUE(*light.CastShadows(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Intensity) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double intensity = 0.2; + lightSdf.SetIntensity(intensity); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(intensity, *light.Intensity(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Direction) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Vector3d dir(1.0, 0.0, 0.0); + lightSdf.SetDirection(dir); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(dir, *light.Direction(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, DiffuseColor) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Color color(1.0, 1.0, 0.0); + lightSdf.SetDiffuse(color); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(color, *light.DiffuseColor(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpecularColor) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Color color(1.0, 1.0, 0.0); + lightSdf.SetSpecular(color); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(color, *light.SpecularColor(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationRange) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double range = 2.4; + lightSdf.SetAttenuationRange(range); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(range, *light.AttenuationRange(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationConstant) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double value = 0.2; + lightSdf.SetConstantAttenuationFactor(value); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(value, *light.AttenuationConstant(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationLinear) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double value = 0.1; + lightSdf.SetLinearAttenuationFactor(value); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(value, *light.AttenuationLinear(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationQuadratic) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double value = 0.01; + lightSdf.SetQuadraticAttenuationFactor(value); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(value, *light.AttenuationQuadratic(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpotInnerAngle) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Angle angle(1.57); + lightSdf.SetSpotInnerAngle(angle); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(angle, *light.SpotInnerAngle(ecm)); +} + + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpotOuterAngle) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Angle angle(2.3); + lightSdf.SetSpotOuterAngle(angle); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(angle, *light.SpotOuterAngle(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpotFalloff) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double falloff(0.3); + lightSdf.SetSpotFalloff(falloff); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(falloff, *light.SpotFalloff(ecm)); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetPose) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Pose3d poseCmd(0.1, -2, 30, 0.2, 0.3, 0.8); + light.SetPose(ecm, poseCmd); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(poseCmd, msgs::Convert( + ecm.Component(eLight)->Data().pose())); + + // Make sure the light cmd is updated + math::Pose3d poseCmd2(9.3, -8, -1, 0.0, -0.3, 3.8); + light.SetPose(ecm, poseCmd2); + EXPECT_EQ(poseCmd2, msgs::Convert( + ecm.Component(eLight)->Data().pose())); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetCastShadows) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + bool castShadows = true; + light.SetCastShadows(ecm, castShadows); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(castShadows, + ecm.Component(eLight)->Data().cast_shadows()); + + // Make sure the light cmd is updated + bool castShadows2 = false; + light.SetCastShadows(ecm, castShadows2); + EXPECT_EQ(castShadows2, + ecm.Component(eLight)->Data().cast_shadows()); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetIntensity) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double intensity = 0.3; + light.SetIntensity(ecm, intensity); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(intensity, + ecm.Component(eLight)->Data().intensity()); + + // Make sure the light cmd is updated + double intensity2 = 0.001; + light.SetIntensity(ecm, intensity2); + EXPECT_FLOAT_EQ(intensity2, + ecm.Component(eLight)->Data().intensity()); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetDirection) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Vector3d dir(0.3, 0.4, 0.6); + light.SetDirection(ecm, dir); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(dir, msgs::Convert( + ecm.Component(eLight)->Data().direction())); + + // Make sure the light cmd is updated + math::Vector3d dir2(1.0, 0.0, 0.0); + light.SetDirection(ecm, dir2); + EXPECT_EQ(dir2, msgs::Convert( + ecm.Component(eLight)->Data().direction())); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetDiffuseColor) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Color color(1.0, 0.0, 1.0); + light.SetDiffuseColor(ecm, color); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(color, msgs::Convert( + ecm.Component(eLight)->Data().diffuse())); + + // Make sure the light cmd is updated + math::Color color2(1.0, 0.5, 0.5); + light.SetDiffuseColor(ecm, color2); + EXPECT_EQ(color2, msgs::Convert( + ecm.Component(eLight)->Data().diffuse())); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpecularColor) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Color color(1.0, 1.0, 0.0); + light.SetSpecularColor(ecm, color); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(color, msgs::Convert( + ecm.Component(eLight)->Data().specular())); + + // Make sure the light cmd is updated + math::Color color2(0.0, 1.0, 0.0); + light.SetSpecularColor(ecm, color2); + EXPECT_EQ(color2, msgs::Convert( + ecm.Component(eLight)->Data().specular())); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationRange) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double range = 56.2; + light.SetAttenuationRange(ecm, range); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(range, + ecm.Component(eLight)->Data().range()); + + // Make sure the light cmd is updated + double range2 = 2777.9; + light.SetAttenuationRange(ecm, range2); + EXPECT_FLOAT_EQ(range2, + ecm.Component(eLight)->Data().range()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationConstant) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double value = 3.0; + light.SetAttenuationConstant(ecm, value); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(value, + ecm.Component(eLight)->Data().attenuation_constant()); + + // Make sure the light cmd is updated + double value2 = 5.0; + light.SetAttenuationConstant(ecm, value2); + EXPECT_FLOAT_EQ(value2, + ecm.Component(eLight)->Data().attenuation_constant()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationLinear) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double value = 0.1; + light.SetAttenuationLinear(ecm, value); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(value, + ecm.Component(eLight)->Data().attenuation_linear()); + + // Make sure the light cmd is updated + double value2 = 0.2; + light.SetAttenuationLinear(ecm, value2); + EXPECT_FLOAT_EQ(value2, + ecm.Component(eLight)->Data().attenuation_linear()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationQuadratic) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double value = 0.3; + light.SetAttenuationQuadratic(ecm, value); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(value, + ecm.Component( + eLight)->Data().attenuation_quadratic()); + + // Make sure the light cmd is updated + double value2 = 0.7; + light.SetAttenuationQuadratic(ecm, value2); + EXPECT_FLOAT_EQ(value2, + ecm.Component( + eLight)->Data().attenuation_quadratic()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpotInnerAngle) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Angle angle(2.9); + light.SetSpotInnerAngle(ecm, angle.Radian()); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(angle.Radian(), + ecm.Component(eLight)->Data().spot_inner_angle()); + + // Make sure the light cmd is updated + math::Angle angle2(0.9); + light.SetSpotInnerAngle(ecm, angle2.Radian()); + EXPECT_FLOAT_EQ(angle2.Radian(), + ecm.Component(eLight)->Data().spot_inner_angle()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpotOuterAngle) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Angle angle(3.1); + light.SetSpotOuterAngle(ecm, angle.Radian()); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(angle.Radian(), + ecm.Component(eLight)->Data().spot_outer_angle()); + + // Make sure the light cmd is updated + math::Angle angle2(0.8); + light.SetSpotOuterAngle(ecm, angle2.Radian()); + EXPECT_FLOAT_EQ(angle2.Radian(), + ecm.Component(eLight)->Data().spot_outer_angle()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpotFalloff) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double falloff = 0.3; + light.SetSpotFalloff(ecm, falloff); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(falloff, + ecm.Component(eLight)->Data().spot_falloff()); + + // Make sure the light cmd is updated + double falloff2 = 1.0; + light.SetSpotFalloff(ecm, falloff2); + EXPECT_FLOAT_EQ(falloff2, + ecm.Component(eLight)->Data().spot_falloff()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Parent) +{ + EntityComponentManager ecm; + + { + // Link as parent + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + EXPECT_FALSE(light.Parent(ecm).has_value()); + + ecm.CreateComponent(eLight, + components::ParentEntity(eLink)); + ASSERT_TRUE(light.Valid(ecm)); + + // Check parent link + EXPECT_EQ(eLink, ecm.ParentEntity(eLight)); + auto parentLink = light.Parent(ecm); + EXPECT_EQ(eLink, parentLink); + } + + { + // World as parent + auto eWorld = ecm.CreateEntity(); + ecm.CreateComponent(eWorld, components::World()); + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + EXPECT_FALSE(light.Parent(ecm).has_value()); + + ecm.CreateComponent(eLight, + components::ParentEntity(eWorld)); + ASSERT_TRUE(light.Valid(ecm)); + + // Check parent world + EXPECT_EQ(eWorld, ecm.ParentEntity(eLight)); + auto parentWorld = light.Parent(ecm); + EXPECT_EQ(eWorld, parentWorld); + } +} diff --git a/test/integration/rf_comms.cc b/test/integration/rf_comms.cc index 38c191a7ed..619c994c87 100644 --- a/test/integration/rf_comms.cc +++ b/test/integration/rf_comms.cc @@ -63,12 +63,11 @@ TEST_F(RFCommsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RFComms)) { // Verify msg content std::lock_guard lock(mutex); - std::string expected = "hello world " + std::to_string(msgCounter); gz::msgs::StringMsg receivedMsg; receivedMsg.ParseFromString(_msg.data()); - EXPECT_EQ(expected, receivedMsg.data()); + EXPECT_NE(std::string::npos, receivedMsg.data().find("hello world")); ASSERT_GT(_msg.header().data_size(), 0); EXPECT_EQ("rssi", _msg.header().data(0).key()); msgCounter++; @@ -108,14 +107,16 @@ TEST_F(RFCommsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RFComms)) server.Run(true, 100, false); } - // Verify subscriber received all msgs. + // there is a non-zero probability that the packet may be lost + // Verify subscriber received most of the msgs. + unsigned int expectedMsgCount = static_cast(pubCount * 0.5); int sleep = 0; bool done = false; while (!done && sleep++ < 10) { std::lock_guard lock(mutex); - done = msgCounter == pubCount; + done = msgCounter > expectedMsgCount; std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - EXPECT_EQ(pubCount, msgCounter); + EXPECT_LT(expectedMsgCount, msgCounter); } diff --git a/test/integration/sensor.cc b/test/integration/sensor.cc new file mode 100644 index 0000000000..b535080aeb --- /dev/null +++ b/test/integration/sensor.cc @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2023 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 "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class SensorIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Sensor sensor; + EXPECT_FALSE(sensor.Valid(ecm)); + } + + // Missing sensor component + { + auto id = ecm.CreateEntity(); + Sensor sensor(id); + EXPECT_FALSE(sensor.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Sensor()); + + Sensor sensor(id); + EXPECT_TRUE(sensor.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Sensor()); + + Sensor sensor(id); + + // No name + EXPECT_EQ(std::nullopt, sensor.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("sensor_name")); + EXPECT_EQ("sensor_name", sensor.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Sensor()); + + Sensor sensor(id); + + // No pose + EXPECT_EQ(std::nullopt, sensor.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, sensor.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Topic) +{ + EntityComponentManager ecm; + + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + + Sensor sensor(eSensor); + + std::string topic = "sensor_topic"; + ecm.CreateComponent(eSensor, + components::SensorTopic(topic)); + + EXPECT_EQ(topic, sensor.Topic(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Parent) +{ + EntityComponentManager ecm; + + { + // Link as parent + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + + Sensor sensor(eSensor); + EXPECT_EQ(eSensor, sensor.Entity()); + EXPECT_FALSE(sensor.Parent(ecm).has_value()); + + ecm.CreateComponent(eSensor, + components::ParentEntity(eLink)); + ASSERT_TRUE(sensor.Valid(ecm)); + + // Check parent link + EXPECT_EQ(eLink, ecm.ParentEntity(eSensor)); + auto parentLink = sensor.Parent(ecm); + EXPECT_EQ(eLink, parentLink); + } + + { + // Joint as parent + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + + Sensor sensor(eSensor); + EXPECT_EQ(eSensor, sensor.Entity()); + EXPECT_FALSE(sensor.Parent(ecm).has_value()); + + ecm.CreateComponent(eSensor, + components::ParentEntity(eJoint)); + ASSERT_TRUE(sensor.Valid(ecm)); + + // Check parent joint + EXPECT_EQ(eJoint, ecm.ParentEntity(eSensor)); + auto parentJoint = sensor.Parent(ecm); + EXPECT_EQ(eJoint, parentJoint); + } +} diff --git a/test/integration/sensors_system.cc b/test/integration/sensors_system.cc index 65a085d0bd..c09e2adf75 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -93,6 +93,39 @@ void testSensorEntityIds(const sim::EntityComponentManager &_ecm, } } +///////////////////////////////////////////////// +void testSensorTopicComponents(const sim::EntityComponentManager &_ecm, + const std::unordered_set &_ids, + const std::vector &_topics) +{ + EXPECT_FALSE(_ids.empty()); + for (const auto & id : _ids) + { + auto sensorTopicComp = _ecm.Component(id); + EXPECT_TRUE(sensorTopicComp); + std::string topicStr = "/" + sensorTopicComp->Data(); + EXPECT_FALSE(topicStr.empty()); + + // verify that the topic string stored in sensor topic component + // exits in the list of topics + // For rendering sensors, they may advertize more than one topics but + // the the sensor topic component will only contain one of them, e.g. + // * /image - stored in sensor topic component + // * /camera_info + bool foundTopic = false; + for (auto it = _topics.begin(); it != _topics.end(); ++it) + { + std::string topic = *it; + if (topic.find(topicStr) == 0u) + { + foundTopic = true; + break; + } + } + EXPECT_TRUE(foundTopic); + } +} + ////////////////////////////////////////////////// class SensorsFixture : public InternalFixture> { @@ -117,21 +150,10 @@ class SensorsFixture : public InternalFixture> }; ////////////////////////////////////////////////// -void testDefaultTopics() +void testDefaultTopics(const std::vector &_topics) { // TODO(anyone) This should be a new test, but running multiple tests with // sensors is not currently working - std::string prefix{"/world/camera_sensor/model/default_topics/"}; - std::vector topics{ - prefix + "link/camera_link/sensor/camera/image", - prefix + "link/camera_link/sensor/camera/camera_info", - prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", - prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", - prefix + "link/depth_camera_link/sensor/depth_camera/camera_info", - prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", - prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image" - }; - std::vector publishers; transport::Node node; @@ -139,14 +161,14 @@ void testDefaultTopics() // time int sleep{0}; int maxSleep{30}; - for (; sleep < maxSleep && !node.TopicInfo(topics.front(), publishers); + for (; sleep < maxSleep && !node.TopicInfo(_topics.front(), publishers); ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } ASSERT_LT(sleep, maxSleep); - for (const std::string &topic : topics) + for (const std::string &topic : _topics) { bool result = node.TopicInfo(topic, publishers); @@ -198,9 +220,30 @@ TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) server.Run(true, 50, false); ASSERT_NE(nullptr, ecm); - testDefaultTopics(); + std::string prefix{"/world/camera_sensor/model/default_topics/"}; + std::vector topics{ + prefix + "link/camera_link/sensor/camera/image", + prefix + "link/camera_link/sensor/camera/camera_info", + prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", + prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", + prefix + "link/depth_camera_link/sensor/depth_camera/camera_info", + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image", + prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", + prefix + "link/thermal_camera_link/sensor/thermal_camera/image", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/colored_map", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/labels_map", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/camera_info", + "/camera" + }; + testDefaultTopics(topics); testSensorEntityIds(*ecm, g_sensorEntityIds); + testSensorTopicComponents(*ecm, g_sensorEntityIds, topics); + g_sensorEntityIds.clear(); g_scene.reset();