diff --git a/vrx_gz/CMakeLists.txt b/vrx_gz/CMakeLists.txt index 9cc92d09e..99c61ad3a 100644 --- a/vrx_gz/CMakeLists.txt +++ b/vrx_gz/CMakeLists.txt @@ -96,6 +96,7 @@ install( # Plugins list(APPEND VRX_GZ_PLUGINS + BallShooterPlugin PerceptionScoringPlugin SimpleHydrodynamics StationkeepingScoringPlugin diff --git a/vrx_gz/src/BallShooterPlugin.cc b/vrx_gz/src/BallShooterPlugin.cc new file mode 100644 index 000000000..fba13115e --- /dev/null +++ b/vrx_gz/src/BallShooterPlugin.cc @@ -0,0 +1,310 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "BallShooterPlugin.hh" + +using namespace gz; +using namespace vrx; + +/// \brief Private BallShooterPlugin data class. +class BallShooterPlugin::Implementation +{ + /// \brief Callback function called when receiving a new fire message. + /// \param[in] _msg Unused. + public: void OnFire(const gz::msgs::Empty &_msg); + + /// \brief Protect some member variables used in the callback. + public: std::mutex mutex; + + /// \brief Gz transport node + public: gz::transport::Node node; + + /// \brief Number of shots allowed. + public: unsigned int remainingShots = math::MAX_UI32; + + /// \brief The force (N) to be applied to the projectile. + public: double shotForce = 250; + + /// \brief Projectile model entity. + public: gz::sim::Model projectileModel; + + /// \brief Projectile link entity. + public: gz::sim::Link projectileLink; + + /// \brief Link/model entity that the projectile pose uses as its frame of + /// reference. + public: gz::sim::Entity frame; + + /// \brief Pose in which the projectile should be placed before launching it. + public: gz::math::Pose3d pose = gz::math::Pose3d::Zero; + + /// \brief Ready to shoot a ball when true. + public: bool shotReady = false; + + /// \brief True to reset ball velocities + public: bool resetVel = false; +}; + +////////////////////////////////////////////////// +BallShooterPlugin::BallShooterPlugin() + : System(), dataPtr(utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +void BallShooterPlugin::Configure(const sim::Entity &_entity, + const std::shared_ptr &_sdf, + sim::EntityComponentManager &_ecm, sim::EventManager &_eventMgr) +{ + auto sdf = _sdf->Clone(); + + // Parse the required element. + if (!sdf->HasElement("projectile")) + { + gzerr << "BallShooterPlugin: Missing element" << std::endl; + return; + } + + sdf::ElementPtr projectileElem = sdf->GetElement("projectile"); + + // Parse the required used as projectile. + if (!projectileElem->HasElement("model_name")) + { + gzerr << "BallShooterPlugin: Missing element\n"; + return; + } + + std::string projectileName = + projectileElem->GetElement("model_name")->Get(); + + sim::Entity projectileModelEntity = _ecm.EntityByComponents( + sim::components::Model(), sim::components::Name(projectileName)); + + if (projectileModelEntity == sim::kNullEntity) + { + gzerr << "BallShooterPlugin: The model '" << projectileName + << "' does not exist" << std::endl; + return; + } + this->dataPtr->projectileModel = sim::Model(projectileModelEntity); + + // Parse the required + if (!projectileElem->HasElement("link_name")) + { + gzerr << "BallShooterPlugin: Missing element\n"; + return; + } + + std::string projectileLinkName = + projectileElem->GetElement("link_name")->Get(); + + sim::Entity projectileLinkEntity = _ecm.EntityByComponents( + sim::components::Link(), sim::components::Name(projectileLinkName), + sim::components::ParentEntity(this->dataPtr->projectileModel.Entity())); + + if (projectileLinkEntity == sim::kNullEntity) + { + gzerr << "BallShooterPlugin: The link '" << projectileLinkName + << "' does not exist within '" << projectileName << "'" << std::endl; + return; + } + this->dataPtr->projectileLink = sim::Link(projectileLinkEntity); + + // Parse if available. + std::string frameName; + if (projectileElem->HasElement("frame")) + { + frameName = projectileElem->Get("frame"); + + this->dataPtr->frame = _ecm.EntityByComponents( + sim::components::Name(frameName)); + + if (this->dataPtr->frame == sim::kNullEntity) + { + gzerr << "The frame '" << frameName << "' does not exist" << std::endl; + frameName = ""; + } + else + { + auto isModel = _ecm.Component( + this->dataPtr->frame); + auto isLink = _ecm.Component(this->dataPtr->frame); + if (isLink) + { + // create world pose comp to be populated by physics if it does not + // exist yet + auto worldPoseComp = _ecm.Component( + this->dataPtr->frame); + if (!worldPoseComp) + { + _ecm.CreateComponent( + this->dataPtr->frame, + sim::components::WorldPose()); + } + } + else if (!isModel) + { + this->dataPtr->frame = sim::kNullEntity; + frameName = ""; + gzerr << " tag must list the name of a link or model" << std::endl; + } + } + } + + // Parse if available. + if (projectileElem->HasElement("pose")) + this->dataPtr->pose = projectileElem->Get("pose"); + + // Parse if available. + std::string topic = "/ball_shooter/fire"; + if (sdf->HasElement("topic")) + topic = sdf->GetElement("topic")->Get(); + + // Parse if available. + if (sdf->HasElement("num_shots")) + { + this->dataPtr->remainingShots = + sdf->GetElement("num_shots")->Get(); + } + + // Parse if available. + if (sdf->HasElement("shot_force")) + this->dataPtr->shotForce = sdf->GetElement("shot_force")->Get(); + + this->dataPtr->node.Subscribe(topic, + &BallShooterPlugin::Implementation::OnFire, this->dataPtr.get()); + + // Debug output. + gzdbg << ": " << projectileName << std::endl; + gzdbg << ": " << projectileLinkName << std::endl; + gzdbg << ": " << frameName << std::endl; + gzdbg << ": " << this->dataPtr->remainingShots << std::endl; + gzdbg << ": " << this->dataPtr->pose.Pos() << " " + << this->dataPtr->pose.Rot().Euler() << std::endl; + gzdbg << ": " << this->dataPtr->shotForce << std::endl; + gzdbg << ": " << topic << std::endl; +} + +////////////////////////////////////////////////// +void BallShooterPlugin::PreUpdate(const sim::UpdateInfo &, + sim::EntityComponentManager &_ecm) + +{ + std::lock_guard lock(this->dataPtr->mutex); + + if (!this->dataPtr->shotReady || + this->dataPtr->projectileModel.Entity() == sim::kNullEntity || + this->dataPtr->projectileLink.Entity() == sim::kNullEntity) + return; + + // reset so the ball does not accumulate vel + // Do this for one physics iteration first. Then fire the shooter in the next + // iteration. + if (!this->dataPtr->resetVel) + { + this->dataPtr->projectileLink.SetLinearVelocity(_ecm, math::Vector3d::Zero); + this->dataPtr->projectileLink.SetAngularVelocity(_ecm, math::Vector3d::Zero); + this->dataPtr->resetVel = true; + return; + } + else + { + // make sure to remove the comp otherwise the physics system will keep + // setting zero vel to the link + _ecm.RemoveComponent( + this->dataPtr->projectileLink.Entity()); + _ecm.RemoveComponent( + this->dataPtr->projectileLink.Entity()); + } + + // Set the new pose of the projectile based on the frame. + math::Pose3d projectilePose = this->dataPtr->pose; + if (this->dataPtr->frame != sim::kNullEntity) + { + // get frame entity's world pose + math::Pose3d framePose; + if (_ecm.Component(this->dataPtr->frame)) + { + framePose = _ecm.Component( + this->dataPtr->frame)->Data(); + } + else if (_ecm.Component(this->dataPtr->frame)) + { + framePose = _ecm.Component( + this->dataPtr->frame)->Data(); + } + + math::Matrix4d transMat(framePose); + math::Matrix4d poseLocal(this->dataPtr->pose); + projectilePose = (transMat * poseLocal).Pose(); + } + + // Teleport the projectile to the ball shooter. + this->dataPtr->projectileModel.SetWorldPoseCmd(_ecm, projectilePose); + + // Ignition! + const auto worldPose = this->dataPtr->projectileLink.WorldPose(_ecm); + auto force = worldPose->Rot().RotateVector( + math::Vector3d(this->dataPtr->shotForce, 0, 0)); + this->dataPtr->projectileLink.AddWorldForce(_ecm, force); + + --this->dataPtr->remainingShots; + this->dataPtr->shotReady = false; + this->dataPtr->resetVel = false; +} + +////////////////////////////////////////////////// +void BallShooterPlugin::Implementation::OnFire(const gz::msgs::Empty &_msg) +{ + std::lock_guard lock(this->mutex); + + if (this->remainingShots <= 0) + { + gzdbg << "BallShooterPlugin: Maximum number of shots already reached. " + << "Request ignored" << std::endl; + return; + } + + this->shotReady = true; +} + +GZ_ADD_PLUGIN(BallShooterPlugin, + sim::System, + BallShooterPlugin::ISystemConfigure, + BallShooterPlugin::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(vrx::BallShooterPlugin, + "vrx::BallShooterPlugin") diff --git a/vrx_gz/src/BallShooterPlugin.hh b/vrx_gz/src/BallShooterPlugin.hh new file mode 100644 index 000000000..72d92c50e --- /dev/null +++ b/vrx_gz/src/BallShooterPlugin.hh @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef VRX_BALL_SHOOTER_PLUGIN_HH_ +#define VRX_BALL_SHOOTER_PLUGIN_HH_ + +#include + +#include +#include +#include +#include + +namespace vrx +{ +/// \brief Simulate a ball shooter. A projectile is launched when a +/// ROS message is received. The ball is reused (teleported) from previous +/// shots. +/// +/// The plugin accepts the following SDF parameters: +/// * Required parameters: +/// The object used as projectile should be declared as a +/// element with the following parameters: +/// * The name of the model used as projectile. +/// Required parameter. +/// * The name of the link within the projectile +/// model where the force will be applied when +/// firing the shooter. Required parameter. +/// * If present, the parameter will be in the +/// frame of this link/model. Otherwise the world +/// frame is used. Optional parameter. +/// * - Pose of the projectile right before being shot. +/// Optional parameter. Default to {0 0 0 0 0 0}. +/// +/// * Optional parameters: +/// - Number of shots allowed. Default to UINT_MAX. +/// - Force (N) applied to the projectile. Default to 250 N. +/// - Name of the ROS topic to shoot. Default to "/ball_shooter/fire". +/// +/// Here's an example: +/// +/// +/// a_projectile +/// link +/// my_robot/ball_shooter_link +/// 0.2 0 0 0 0 0 +/// +/// 250 +/// my_robot/ball_shooter/fire +/// +class BallShooterPlugin + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate +{ + // \brief Constructor. + public: BallShooterPlugin(); + + /// \brief Destructor. + public: ~BallShooterPlugin() override = default; + + // Documentation inherited. + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; + + // Documentation inherited. + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer. + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) +}; +} +#endif diff --git a/vrx_gz/src/vrx_gz/payload_bridges.py b/vrx_gz/src/vrx_gz/payload_bridges.py index 7a1112232..20dc8f249 100644 --- a/vrx_gz/src/vrx_gz/payload_bridges.py +++ b/vrx_gz/src/vrx_gz/payload_bridges.py @@ -126,6 +126,14 @@ def odometry(model_name): ros_type='nav_msgs/msg/Odometry', direction=BridgeDirection.GZ_TO_ROS) +def ball_shooter(model_name): + return Bridge( + gz_topic=f'/{model_name}/shooters/ball_shooter/fire', + ros_topic=f'shooters/ball_shooter/fire', + gz_type='ignition.msgs.Empty', + ros_type='std_msgs/msg/Empty', + direction=BridgeDirection.ROS_TO_GZ) + def payload_bridges(world_name, model_name, link_name, sensor_name, sensor_type): bridges = [] @@ -162,4 +170,9 @@ def payload_bridges(world_name, model_name, link_name, sensor_name, sensor_type) bridges = [ odometry(model_name), ] + elif 'BallShooter' in sensor_name: + bridges = [ + ball_shooter(model_name), + ] + return bridges diff --git a/vrx_gz/worlds/perception_task.sdf b/vrx_gz/worlds/perception_task.sdf index 538babd8a..ec492279d 100644 --- a/vrx_gz/worlds/perception_task.sdf +++ b/vrx_gz/worlds/perception_task.sdf @@ -400,11 +400,43 @@ - + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 -0.02 0 0 0 + + + 0.0285 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + 0 0 0 diff --git a/vrx_gz/worlds/stationkeeping_task.sdf b/vrx_gz/worlds/stationkeeping_task.sdf index 4b223b1fd..6c43da7f3 100644 --- a/vrx_gz/worlds/stationkeeping_task.sdf +++ b/vrx_gz/worlds/stationkeeping_task.sdf @@ -638,11 +638,43 @@ - + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 -0.02 0 0 0 + + + 0.0285 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + 0 0 0 diff --git a/vrx_gz/worlds/sydney_regatta.sdf b/vrx_gz/worlds/sydney_regatta.sdf index 85ca037a8..161efba8d 100644 --- a/vrx_gz/worlds/sydney_regatta.sdf +++ b/vrx_gz/worlds/sydney_regatta.sdf @@ -638,11 +638,43 @@ - + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 -0.02 0 0 0 + + + 0.0285 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + 0 0 0 diff --git a/vrx_gz/worlds/wayfinding_task.sdf b/vrx_gz/worlds/wayfinding_task.sdf index c8d905460..7b1be7d5b 100644 --- a/vrx_gz/worlds/wayfinding_task.sdf +++ b/vrx_gz/worlds/wayfinding_task.sdf @@ -638,11 +638,43 @@ - + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 -0.02 0 0 0 + + + 0.0285 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + 0 0 0 diff --git a/vrx_gz/worlds/wildlife_task.sdf b/vrx_gz/worlds/wildlife_task.sdf index 43b917d6c..7e58a10cf 100644 --- a/vrx_gz/worlds/wildlife_task.sdf +++ b/vrx_gz/worlds/wildlife_task.sdf @@ -553,11 +553,43 @@ - + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 -0.02 0 0 0 + + + 0.0285 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + 0 0 0 diff --git a/vrx_urdf/vrx_gazebo/models/blue_projectile/model.config b/vrx_urdf/vrx_gazebo/models/blue_projectile/model.config new file mode 100644 index 000000000..94cc16dd8 --- /dev/null +++ b/vrx_urdf/vrx_gazebo/models/blue_projectile/model.config @@ -0,0 +1,16 @@ + + + blue_projectile + 1.0 + model.sdf + + + Carlos Agüero + caguero@openrobotics.org + + + + A blue projectile to be used with the ball shooter. + Reference: https://bit.ly/3jaJDzs + + diff --git a/vrx_urdf/vrx_gazebo/models/blue_projectile/model.sdf b/vrx_urdf/vrx_gazebo/models/blue_projectile/model.sdf new file mode 100644 index 000000000..64bd30b50 --- /dev/null +++ b/vrx_urdf/vrx_gazebo/models/blue_projectile/model.sdf @@ -0,0 +1,38 @@ + + + + + + 0.04 + + + 0.00002166 + 0 + 0 + 00002166 + 0 + 00002166 + + + + + + 0.0285 + + + + + + + 0.0285 + + + + 0.3176 0.6118 0.8353 + 0.3176 0.6118 0.8353 + 0.3176 0.6118 0.8353 + + + + + diff --git a/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro b/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro index fdb3bef0b..fc65e13b6 100644 --- a/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro +++ b/vrx_urdf/wamv_gazebo/urdf/components/ball_shooter.xacro @@ -61,9 +61,9 @@ -