Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add package dropping for simulating drone delivery #724

Merged
merged 1 commit into from
Mar 8, 2021
Merged

Add package dropping for simulating drone delivery #724

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,3 +27,4 @@ models/tiltrotor/tiltrotor.sdf
models/boat/boat.sdf
models/typhoon_h480/typhoon_h480.sdf
models/depth_camera/depth_camera.sdf
models/standard_vtol_drop/standard_vtol_drop.sdf
2 changes: 2 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -357,6 +357,7 @@ add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp)
add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp)
add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp)
add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp)
add_library(gazebo_drop_plugin SHARED src/gazebo_drop_plugin.cpp)

set(plugins
gazebo_airspeed_plugin
Expand All @@ -383,6 +384,7 @@ set(plugins
gazebo_usv_dynamics_plugin
gazebo_parachute_plugin
gazebo_airship_dynamics_plugin
gazebo_drop_plugin
)

foreach(plugin ${plugins})
Expand Down
105 changes: 105 additions & 0 deletions include/gazebo_drop_plugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @brief Drop Plugin
*
* This plugin simulates dropping a payload
*
* @author Jaeyoung Lim <[email protected]>
*/

#ifndef _GAZEBO_DROP_PLUGIN_HH_
#define _GAZEBO_DROP_PLUGIN_HH_

#include <math.h>
#include <common.h>
#include <sdf/sdf.hh>

#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/util/system.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <ignition/math.hh>
#include "CommandMotorSpeed.pb.h"


#include <Odometry.pb.h>

namespace gazebo
{

typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> CommandMotorSpeedPtr;


class GAZEBO_VISIBLE DropPlugin : public ModelPlugin
{
public:
DropPlugin();
virtual ~DropPlugin();

void Load(physics::ModelPtr model, sdf::ElementPtr sdf);
void OnUpdate(const common::UpdateInfo&);
physics::ModelPtr GetModelPtr(std::string model_name);

private:
/// \brief Loads the package model
void LoadPackage();

/// \brief Callback for subscribing to motor commands
/// \param rot_velocities Motor command velocity commanded from firmware
void VelocityCallback(CommandMotorSpeedPtr &rot_velocities);


std::string namespace_;
physics::ModelPtr model_;
physics::WorldPtr world_;
event::ConnectionPtr update_connection_;

bool package_spawned = false; ///< Check if the package has already been spawned in the world
double max_rot_velocity_ = 3500; ///< Clip maximum motor velocity
double ref_motor_rot_vel_ = 0; ///< Reference motor velocity
double release_rot_vel_ = 300; ///< Motor velocity command to release the package
int motor_number_;

std::string trigger_sub_topic_ = "/gazebo/command/motor_speed"; ///< Parachute trigger signal topic
std::string model_name_ = "parachute_package";

transport::NodePtr node_handle_;
transport::SubscriberPtr trigger_sub_;

}; // class GAZEBO_VISIBLE DropPlugin
} // namespace gazebo
#endif // _GAZEBO_DROP_PLUGIN_HH_
16 changes: 16 additions & 0 deletions models/parachute_package/model.config
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
<?xml version="1.0"?>

<model>
<name>Parachute Package</name>
<version>1.0</version>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>Jaeyoung Lim</name>
<email>[email protected]</email>
</author>

<description>
A package that has a parachute attached, so it can be dropped from a vehicle
</description>

</model>
67 changes: 67 additions & 0 deletions models/parachute_package/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="parachute_package">
<include>
<uri>model://parachute_small</uri>
<pose>0 0 0.2 0 0 0</pose>
</include>
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>1.00</mass>
<inertia>
<ixx>0.010000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>0.010000</iyy>
<iyz>0.000000</iyz>
<izz>0.010000</izz>
</inertia>
</inertial>
<collision name="collision">
<pose>0 0 0.3 0 0 0</pose>
<geometry>
<box>
<size>0.15 0.1 0.18</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<max_vel>0.1</max_vel>
<min_depth>0.001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name="visual">
<pose>0 0 0.3 0 0 0</pose>
<geometry>
<mesh>
<scale>0.125 0.125 0.125</scale>
<uri>model://big_box/meshes/big_box.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='parachute_joint' type='revolute'>
<child>base_link</child>
<parent>parachute_small::chute</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>
15 changes: 15 additions & 0 deletions models/standard_vtol_drop/model.config
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<model>
<name>Standard VTOL drop</name>
<version>1.0</version>
<sdf version='1.4'>standard_vtol_drop.sdf</sdf>

<author>
<name>Jaeyoung Lim</name>
<email>[email protected]</email>
</author>

<description>
This is a model of a standard VTOL quad plane.
</description>
</model>
Loading