submit hector_quadrotor and rosflight_ws

Flight_control/src/hector_quadrotor/hector_quadrotor.rosinstall

@@ -0,0 +1,4 @@
+- git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: indigo-devel}
+- git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin}
+- git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: indigo-devel}
+- git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: indigo-devel}

Flight_control/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst

@@ -0,0 +1,26 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_quadrotor
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.5 (2015-03-28)
+------------------
+
+0.3.4 (2015-02-22)
+------------------
+
+0.3.3 (2014-09-01)
+------------------
+* moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo
+* Contributors: Johannes Meyer
+
+0.3.2 (2014-03-30)
+------------------
+* added packages hector_quadrotor_controller and hector_quadrotor_controller_gazebo to meta-package
+* Contributors: Johannes Meyer
+
+0.3.1 (2013-12-26)
+------------------
+
+0.3.0 (2013-09-11)
+------------------
+* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack

Flight_control/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hector_quadrotor)
+find_package(catkin REQUIRED)
+catkin_metapackage()

Flight_control/src/hector_quadrotor/hector_quadrotor/package.xml

@@ -0,0 +1,30 @@
+<package>
+  <name>hector_quadrotor</name>
+  <version>0.3.5</version>
+  <description>hector_quadrotor contains packages related to modeling, control and simulation of quadrotor UAV systems</description>
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_quadrotor</url>
+  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
+
+  <author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>hector_quadrotor_controller</run_depend>
+  <run_depend>hector_quadrotor_description</run_depend>
+  <run_depend>hector_quadrotor_model</run_depend>
+  <run_depend>hector_quadrotor_teleop</run_depend>
+  <run_depend>hector_uav_msgs</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+
+</package>

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CHANGELOG.rst

@@ -0,0 +1,46 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_quadrotor_controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.5 (2015-03-28)
+------------------
+* updated angular/z controller parameters
+* Remove redundant callback queue in twist_controller, use root_nh queue as per ros_control API
+* Add controller timeout to allow faster shutdown of spawner
+* Contributors: Johannes Meyer, Paul Bovbel
+
+0.3.4 (2015-02-22)
+------------------
+* improved automatic landing detection and shutdown on rollovers
+* slightly updated velocity controller limits and gains
+* Contributors: Johannes Meyer
+
+0.3.3 (2014-09-01)
+------------------
+* fixed some compiler warnings and missing return values
+* increased integral gain for attitude stabilization (fix #12)
+* make a copy of the root NodeHandle in all controllers
+  For some reason deconstructing the TwistController resulted in a pure virtual function call without this patch.
+* Contributors: Johannes Meyer
+
+0.3.2 (2014-03-30)
+------------------
+* Fix boost 1.53 issues
+  changed boost::shared_dynamic_cast to boost::dynamic_pointer_cast and
+  boost::shared_static_cast to boost::static_pointer_cast
+* use a separate callback queue thread for the TwistController
+* added optional twist limit in pose controller
+* Contributors: Christopher Hrabia, Johannes Meyer
+
+0.3.1 (2013-12-26)
+------------------
+* New controller implementation using ros_control
+* Added pose controller
+* Added motor controller that controls motor voltages from wrench commands
+* Contributors: Johannes Meyer
+
+0.3.0 (2013-09-11)
+------------------
+* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack
+* Propulsion and aerodynamics models are in hector_quadrotor_model package now.
+* Gazebo plugins are in hector_quadrotor_gazebo_plugins package now.

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hector_quadrotor_controller)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface)
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES hector_quadrotor_controller
+    CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface
+    DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+add_library(hector_quadrotor_controller src/quadrotor_interface.cpp src/pid.cpp)
+target_link_libraries(hector_quadrotor_controller ${catkin_LIBRARIES})
+add_dependencies(hector_quadrotor_controller hector_uav_msgs_generate_messages_cpp)
+
+add_library(hector_quadrotor_pose_controller src/pose_controller.cpp)
+target_link_libraries(hector_quadrotor_pose_controller hector_quadrotor_controller)
+
+add_library(hector_quadrotor_twist_controller src/twist_controller.cpp)
+target_link_libraries(hector_quadrotor_twist_controller hector_quadrotor_controller ${BOOST_LIBRARIES})
+
+add_library(hector_quadrotor_motor_controller src/motor_controller.cpp)
+target_link_libraries(hector_quadrotor_motor_controller hector_quadrotor_controller)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executables and/or libraries for installation
+install(TARGETS hector_quadrotor_controller hector_quadrotor_pose_controller hector_quadrotor_twist_controller hector_quadrotor_motor_controller
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY launch params DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(FILES
+  plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/handles.h

@@ -0,0 +1,536 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#ifndef HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
+#define HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
+
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Wrench.h>
+#include <sensor_msgs/Imu.h>
+#include <hector_uav_msgs/MotorStatus.h>
+#include <hector_uav_msgs/MotorCommand.h>
+#include <hector_uav_msgs/AttitudeCommand.h>
+#include <hector_uav_msgs/YawrateCommand.h>
+#include <hector_uav_msgs/ThrustCommand.h>
+
+namespace hector_quadrotor_controller {
+
+class QuadrotorInterface;
+
+using geometry_msgs::Pose;
+using geometry_msgs::Point;
+using geometry_msgs::Quaternion;
+using geometry_msgs::Twist;
+using geometry_msgs::Vector3;
+using geometry_msgs::Wrench;
+using sensor_msgs::Imu;
+using hector_uav_msgs::MotorStatus;
+using hector_uav_msgs::MotorCommand;
+using hector_uav_msgs::AttitudeCommand;
+using hector_uav_msgs::YawrateCommand;
+using hector_uav_msgs::ThrustCommand;
+
+template <class Derived, typename T>
+class Handle_
+{
+public:
+  typedef T ValueType;
+  typedef Handle_<Derived, T> Base;
+
+  Handle_(const std::string& name, const std::string& field = std::string()) : interface_(0), name_(name), field_(field), value_(0) {}
+  Handle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field), value_(0) {}
+  Handle_(QuadrotorInterface *interface, const ValueType *source, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field) { *this = source; }
+  virtual ~Handle_() {}
+
+  virtual const std::string& getName() const { return name_; }
+  virtual const std::string& getField() const { return field_; }
+
+  virtual bool connected() const { return get(); }
+  virtual void reset() { value_ = 0; }
+
+  Derived& operator=(const ValueType *source) { value_ = source; return static_cast<Derived &>(*this); }
+  const ValueType *get() const { return value_; }
+  const ValueType &operator*() const { return *value_; }
+
+protected:
+  QuadrotorInterface *interface_;
+  const std::string name_;
+  const std::string field_;
+  const ValueType *value_;
+};
+
+class PoseHandle : public Handle_<PoseHandle, Pose>
+{
+public:
+  using Base::operator=;
+
+  PoseHandle() : Base("pose") {}
+  PoseHandle(QuadrotorInterface *interface) : Base(interface, "pose") {}
+  PoseHandle(QuadrotorInterface *interface, const Pose *pose) : Base(interface, pose, "pose") {}
+  virtual ~PoseHandle() {}
+
+  const ValueType& pose() const { return *get(); }
+
+  void getEulerRPY(double &roll, double &pitch, double &yaw) const;
+  double getYaw() const;
+  Vector3 toBody(const Vector3& nav) const;
+  Vector3 fromBody(const Vector3& nav) const;
+};
+typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
+
+class TwistHandle : public Handle_<TwistHandle, Twist>
+{
+public:
+  using Base::operator=;
+
+  TwistHandle() : Base("twist") {}
+  TwistHandle(QuadrotorInterface *interface) : Base(interface, "twist") {}
+  TwistHandle(QuadrotorInterface *interface, const Twist *twist) : Base(interface, twist, "twist") {}
+  virtual ~TwistHandle() {}
+
+  const ValueType& twist() const { return *get(); }
+};
+typedef boost::shared_ptr<TwistHandle> TwistHandlePtr;
+
+class AccelerationHandle : public Handle_<AccelerationHandle, Vector3>
+{
+public:
+  using Base::operator=;
+
+  AccelerationHandle(QuadrotorInterface *interface, const Vector3 *acceleration) : Base(interface, acceleration, "acceleration") {}
+  virtual ~AccelerationHandle() {}
+
+  const ValueType& acceleration() const { return *get(); }
+};
+typedef boost::shared_ptr<AccelerationHandle> AccelerationHandlePtr;
+
+class StateHandle : public PoseHandle, public TwistHandle
+{
+public:
+  StateHandle() {}
+  StateHandle(QuadrotorInterface *interface, const Pose *pose, const Twist *twist) : PoseHandle(interface, pose), TwistHandle(interface, twist) {}
+  virtual ~StateHandle() {}
+
+  virtual bool connected() const { return PoseHandle::connected() && TwistHandle::connected(); }
+};
+typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
+
+class ImuHandle : public Handle_<ImuHandle, Imu>
+{
+public:
+  using Base::operator=;
+
+  ImuHandle() : Base("imu") {}
+  ImuHandle(QuadrotorInterface *interface, const Imu *imu) : Base(interface, imu, "imu") {}
+  virtual ~ImuHandle() {}
+
+  const ValueType& imu() const { return *get(); }
+};
+typedef boost::shared_ptr<ImuHandle> ImuHandlePtr;
+
+class MotorStatusHandle : public Handle_<MotorStatusHandle, MotorStatus>
+{
+public:
+  using Base::operator=;
+
+  MotorStatusHandle() : Base("motor_status") {}
+  MotorStatusHandle(QuadrotorInterface *interface, const MotorStatus *motor_status) : Base(interface, motor_status, "motor_status") {}
+  virtual ~MotorStatusHandle() {}
+
+  const ValueType& motorStatus() const { return *get(); }
+};
+typedef boost::shared_ptr<MotorStatusHandle> MotorStatusHandlePtr;
+
+class CommandHandle
+{
+public:
+  CommandHandle() : interface_(0), new_value_(false) {}
+  CommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field) : interface_(interface), name_(name), field_(field), new_value_(false) {}
+  virtual ~CommandHandle() {}
+
+  virtual const std::string& getName() const { return name_; }
+  virtual const std::string& getField() const { return field_; }
+  virtual bool connected() const = 0;
+  virtual void reset() {}
+
+  void *get() const { return 0; }
+
+  bool enabled();
+  bool start();
+  void stop();
+  void disconnect();
+
+  template <typename T> T* ownData(T* data) { my_.reset(data); return data; }
+
+  template <typename Derived> bool connectFrom(const Derived& output) {
+    Derived *me = dynamic_cast<Derived *>(this);
+    if (!me) return false;
+    ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", output.getName().c_str(), &output, me->getName().c_str(), me);
+    return (*me = output.get()).connected();
+  }
+
+  template <typename Derived> bool connectTo(Derived& input) const {
+    const Derived *me = dynamic_cast<const Derived *>(this);
+    if (!me) return false;
+    ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", me->getName().c_str(), me, input.getName().c_str(), &input);
+    return (input = me->get()).connected();
+  }
+
+private:
+  QuadrotorInterface *interface_;
+  const std::string name_;
+  const std::string field_;
+  boost::shared_ptr<void> my_;
+
+protected:
+  mutable bool new_value_;
+  bool wasNew() const { bool old = new_value_; new_value_ = false; return old; }
+};
+typedef boost::shared_ptr<CommandHandle> CommandHandlePtr;
+
+namespace internal {
+  template <class Derived> struct FieldAccessor {
+    static typename Derived::ValueType *get(void *) { return 0; }
+  };
+}
+
+template <class Derived, typename T, class Parent = CommandHandle>
+class CommandHandle_ : public Parent
+{
+public:
+  typedef T ValueType;
+  typedef CommandHandle_<Derived, T, Parent> Base;
+
+  CommandHandle_() : command_(0) {}
+  CommandHandle_(const Parent &other) : Parent(other), command_(0) {}
+  CommandHandle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Parent(interface, name, field), command_(0) {}
+  virtual ~CommandHandle_() {}
+
+  virtual bool connected() const { return get(); }
+  virtual void reset() { command_ = 0; Parent::reset(); }
+
+  using Parent::operator=;
+  Derived& operator=(ValueType *source) { command_ = source; return static_cast<Derived &>(*this); }
+  ValueType *get() const { return command_ ? command_ : internal::FieldAccessor<Derived>::get(Parent::get()); }
+  ValueType &operator*() const { return *command_; }
+
+  ValueType& command() { return *get(); }
+  const ValueType& getCommand() const { this->new_value_ = false; return *get(); }
+  void setCommand(const ValueType& command) { this->new_value_ = true; *get() = command; }
+  bool getCommand(ValueType& command) const { command = *get(); return this->wasNew(); }
+
+  bool update(ValueType& command) const {
+    if (!connected()) return false;
+    command = getCommand();
+    return true;
+  }
+
+protected:
+  ValueType *command_;
+};
+
+class PoseCommandHandle : public CommandHandle_<PoseCommandHandle, Pose>
+{
+public:
+  using Base::operator=;
+
+  PoseCommandHandle() {}
+  PoseCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
+  PoseCommandHandle(Pose* command) { *this = command; }
+  virtual ~PoseCommandHandle() {}
+};
+typedef boost::shared_ptr<PoseCommandHandle> PoseCommandHandlePtr;
+
+class HorizontalPositionCommandHandle : public CommandHandle_<HorizontalPositionCommandHandle, Point, PoseCommandHandle>
+{
+public:
+  using Base::operator=;
+
+  HorizontalPositionCommandHandle() {}
+  HorizontalPositionCommandHandle(const PoseCommandHandle& other) : Base(other) {}
+  HorizontalPositionCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.xy") {}
+  HorizontalPositionCommandHandle(Point* command) { *this = command; }
+  virtual ~HorizontalPositionCommandHandle() {}
+
+  using Base::getCommand;
+  virtual bool getCommand(double &x, double &y) const {
+    x = get()->x;
+    y = get()->y;
+    return wasNew();
+  }
+
+  using Base::setCommand;
+  virtual void setCommand(double x, double y)
+  {
+    this->new_value_ = true;
+    get()->x = x;
+    get()->y = y;
+  }
+
+  using Base::update;
+  bool update(Pose& command) const {
+    if (!connected()) return false;
+    getCommand(command.position.x, command.position.y);
+    return true;
+  }
+
+  void getError(const PoseHandle &pose, double &x, double &y) const;
+};
+typedef boost::shared_ptr<HorizontalPositionCommandHandle> HorizontalPositionCommandHandlePtr;
+
+namespace internal {
+  template <> struct FieldAccessor<HorizontalPositionCommandHandle> {
+    static Point *get(Pose *pose) { return &(pose->position); }
+  };
+}
+
+class HeightCommandHandle : public CommandHandle_<HeightCommandHandle, double, PoseCommandHandle>
+{
+public:
+  using Base::operator=;
+
+  HeightCommandHandle() {}
+  HeightCommandHandle(const PoseCommandHandle& other) : Base(other) {}
+  HeightCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.z") {}
+  HeightCommandHandle(double *command) { *this = command; }
+  virtual ~HeightCommandHandle() {}
+
+  using Base::update;
+  bool update(Pose& command) const {
+    if (!connected()) return false;
+    command.position.z = getCommand();
+    return true;
+  }
+
+  double getError(const PoseHandle &pose) const;
+};
+typedef boost::shared_ptr<HeightCommandHandle> HeightCommandHandlePtr;
+
+namespace internal {
+  template <> struct FieldAccessor<HeightCommandHandle> {
+    static double *get(Pose *pose) { return &(pose->position.z); }
+  };
+}
+
+class HeadingCommandHandle : public CommandHandle_<HeadingCommandHandle, Quaternion, PoseCommandHandle>
+{
+public:
+  using Base::operator=;
+
+  HeadingCommandHandle() {}
+  HeadingCommandHandle(const PoseCommandHandle& other) : Base(other), scalar_(0) {}
+  HeadingCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "orientation.yaw"), scalar_(0) {}
+  HeadingCommandHandle(Quaternion *command) { *this = command; }
+  virtual ~HeadingCommandHandle() {}
+
+  using Base::getCommand;
+  double getCommand() const;
+
+  using Base::setCommand;
+  void setCommand(double command);
+
+  using Base::update;
+  bool update(Pose& command) const;
+
+  double getError(const PoseHandle &pose) const;
+
+protected:
+  double *scalar_;
+};
+typedef boost::shared_ptr<HeadingCommandHandle> HeadingCommandHandlePtr;
+
+namespace internal {
+  template <> struct FieldAccessor<HeadingCommandHandle> {
+    static Quaternion *get(Pose *pose) { return &(pose->orientation); }
+  };
+}
+
+class TwistCommandHandle : public CommandHandle_<TwistCommandHandle, Twist>
+{
+public:
+
+  using Base::operator=;
+
+  TwistCommandHandle() {}
+  TwistCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
+  TwistCommandHandle(Twist* command) { *this = command; }
+  virtual ~TwistCommandHandle() {}
+};
+typedef boost::shared_ptr<TwistCommandHandle> TwistCommandHandlePtr;
+
+class HorizontalVelocityCommandHandle : public CommandHandle_<HorizontalVelocityCommandHandle, Vector3, TwistCommandHandle>
+{
+public:
+  using Base::operator=;
+
+  HorizontalVelocityCommandHandle() {}
+  HorizontalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
+  HorizontalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.xy") {}
+  HorizontalVelocityCommandHandle(Vector3* command) { *this = command; }
+  virtual ~HorizontalVelocityCommandHandle() {}
+
+  using Base::getCommand;
+  bool getCommand(double &x, double &y) const {
+    x = get()->x;
+    y = get()->y;
+    return true;
+  }
+
+  using Base::setCommand;
+  void setCommand(double x, double y)
+  {
+    get()->x = x;
+    get()->y = y;
+  }
+
+  using Base::update;
+  bool update(Twist& command) const {
+    if (!connected()) return false;
+    getCommand(command.linear.x, command.linear.y);
+    return true;
+  }
+};
+typedef boost::shared_ptr<HorizontalVelocityCommandHandle> HorizontalVelocityCommandHandlePtr;
+
+namespace internal {
+  template <> struct FieldAccessor<HorizontalVelocityCommandHandle> {
+    static Vector3 *get(Twist *twist) { return &(twist->linear); }
+  };
+}
+
+class VerticalVelocityCommandHandle : public CommandHandle_<VerticalVelocityCommandHandle, double, TwistCommandHandle>
+{
+public:
+  using Base::operator=;
+
+  VerticalVelocityCommandHandle() {}
+  VerticalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
+  VerticalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.z") {}
+  VerticalVelocityCommandHandle(double* command) { *this = command; }
+  virtual ~VerticalVelocityCommandHandle() {}
+
+  using Base::update;
+  bool update(Twist& command) const {
+    if (!connected()) return false;
+    command.linear.z = *get();
+    return true;
+  }
+};
+typedef boost::shared_ptr<VerticalVelocityCommandHandle> VerticalVelocityCommandHandlePtr;
+
+namespace internal {
+  template <> struct FieldAccessor<VerticalVelocityCommandHandle> {
+    static double *get(Twist *twist) { return &(twist->linear.z); }
+  };
+}
+
+class AngularVelocityCommandHandle : public CommandHandle_<AngularVelocityCommandHandle, double, TwistCommandHandle>
+{
+public:
+  using Base::operator=;
+
+  AngularVelocityCommandHandle() {}
+  AngularVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
+  AngularVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "angular.z") {}
+  AngularVelocityCommandHandle(double* command) { *this = command; }
+  virtual ~AngularVelocityCommandHandle() {}
+
+  using Base::update;
+  bool update(Twist& command) const {
+    if (!connected()) return false;
+    command.linear.z = *get();
+    return true;
+  }
+};
+typedef boost::shared_ptr<AngularVelocityCommandHandle> AngularVelocityCommandHandlePtr;
+
+namespace internal {
+  template <> struct FieldAccessor<AngularVelocityCommandHandle> {
+    static double *get(Twist *twist) { return &(twist->angular.z); }
+  };
+}
+
+class WrenchCommandHandle : public CommandHandle_<WrenchCommandHandle, Wrench>
+{
+public:
+  using Base::operator=;
+
+  WrenchCommandHandle() {}
+  WrenchCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
+  virtual ~WrenchCommandHandle() {}
+};
+typedef boost::shared_ptr<WrenchCommandHandle> WrenchCommandHandlePtr;
+
+class MotorCommandHandle : public CommandHandle_<MotorCommandHandle, MotorCommand>
+{
+public:
+  using Base::operator=;
+
+  MotorCommandHandle() {}
+  MotorCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
+  virtual ~MotorCommandHandle() {}
+};
+typedef boost::shared_ptr<MotorCommandHandle> MotorCommandHandlePtr;
+
+class AttitudeCommandHandle : public CommandHandle_<AttitudeCommandHandle, AttitudeCommand>
+{
+public:
+  using Base::operator=;
+
+  AttitudeCommandHandle() {}
+  AttitudeCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
+  virtual ~AttitudeCommandHandle() {}
+};
+typedef boost::shared_ptr<AttitudeCommandHandle> AttitudeCommandHandlePtr;
+
+class YawrateCommandHandle : public CommandHandle_<YawrateCommandHandle, YawrateCommand>
+{
+public:
+  using Base::operator=;
+
+  YawrateCommandHandle() {}
+  YawrateCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
+  virtual ~YawrateCommandHandle() {}
+};
+typedef boost::shared_ptr<YawrateCommandHandle> YawrateCommandHandlePtr;
+
+class ThrustCommandHandle : public CommandHandle_<ThrustCommandHandle, ThrustCommand>
+{
+public:
+  using Base::operator=;
+
+  ThrustCommandHandle() {}
+  ThrustCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
+  virtual ~ThrustCommandHandle() {}
+};
+typedef boost::shared_ptr<ThrustCommandHandle> ThrustCommandHandlePtr;
+
+} // namespace hector_quadrotor_controller
+
+#endif // HECTOR_QUADROTOR_CONTROLLER_HANDLES_H

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h

@@ -0,0 +1,45 @@
+#ifndef HECTOR_QUADROTOR_CONTROLLER_PID_H
+#define HECTOR_QUADROTOR_CONTROLLER_PID_H
+
+#include <ros/node_handle.h>
+
+namespace hector_quadrotor_controller {
+
+class PID
+{
+public:
+  struct parameters {
+    parameters();
+    bool enabled;
+    double time_constant;
+    double k_p;
+    double k_i;
+    double k_d;
+    double limit_i;
+    double limit_output;
+  } parameters_;
+
+  struct state {
+    state();
+    double p, i, d;
+    double input, dinput;
+    double dx;
+  } state_;
+
+public:
+  PID();
+  PID(const parameters& parameters);
+  ~PID();
+
+  void init(const ros::NodeHandle &param_nh);
+  void reset();
+
+  double update(double input, double x, double dx, const ros::Duration& dt);
+  double update(double error, double dx, const ros::Duration& dt);
+
+  double getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt);
+};
+
+} // namespace hector_quadrotor_controller
+
+#endif // HECTOR_QUADROTOR_CONTROLLER_PID_H

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/quadrotor_interface.h

@@ -0,0 +1,143 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
+#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
+
+#include <hardware_interface/internal/hardware_resource_manager.h>
+
+#include <hector_quadrotor_controller/handles.h>
+
+#include <map>
+#include <list>
+
+namespace hector_quadrotor_controller {
+
+using namespace hardware_interface;
+
+class QuadrotorInterface : public HardwareInterface
+{
+public:
+  QuadrotorInterface();
+  virtual ~QuadrotorInterface();
+
+  virtual PoseHandlePtr getPose()                 { return PoseHandlePtr(); }
+  virtual TwistHandlePtr getTwist()               { return TwistHandlePtr(); }
+  virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(); }
+  virtual ImuHandlePtr getSensorImu()             { return ImuHandlePtr(); }
+  virtual MotorStatusHandlePtr getMotorStatus()   { return MotorStatusHandlePtr(); }
+
+  virtual bool getMassAndInertia(double &mass, double inertia[3]) { return false; }
+
+  template <typename HandleType> boost::shared_ptr<HandleType> getHandle()
+  {
+    return boost::shared_ptr<HandleType>(new HandleType(this));
+  }
+
+  template <typename HandleType> boost::shared_ptr<HandleType> addInput(const std::string& name)
+  {
+    boost::shared_ptr<HandleType> input = getInput<HandleType>(name);
+    if (input) return input;
+
+    // create new input handle
+    input.reset(new HandleType(this, name));
+    inputs_[name] = input;
+
+    // connect to output of same name
+    if (outputs_.count(name)) {
+      boost::shared_ptr<HandleType> output = boost::dynamic_pointer_cast<HandleType>(outputs_.at(name));
+      output->connectTo(*input);
+    }
+
+    return input;
+  }
+
+  template <typename HandleType> boost::shared_ptr<HandleType> addOutput(const std::string& name)
+  {
+    boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
+    if (output) return output;
+
+    // create new output handle
+    output.reset(new HandleType(this, name));
+    outputs_[name] = output;
+    *output = output->ownData(new typename HandleType::ValueType());
+
+    //claim resource
+    claim(name);
+
+    // connect to output of same name
+    if (inputs_.count(name)) {
+      boost::shared_ptr<HandleType> input = boost::dynamic_pointer_cast<HandleType>(inputs_.at(name));
+      output->connectTo(*input);
+    }
+
+    return output;
+  }
+
+  template <typename HandleType> boost::shared_ptr<HandleType> getOutput(const std::string& name) const
+  {
+    if (!outputs_.count(name)) return boost::shared_ptr<HandleType>();
+    return boost::static_pointer_cast<HandleType>(outputs_.at(name));
+  }
+
+  template <typename HandleType> boost::shared_ptr<HandleType> getInput(const std::string& name) const
+  {
+    if (!inputs_.count(name)) return boost::shared_ptr<HandleType>();
+    return boost::static_pointer_cast<HandleType>(inputs_.at(name));
+  }
+
+  template <typename HandleType> typename HandleType::ValueType const* getCommand(const std::string& name) const
+  {
+    boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
+    if (!output || !output->connected()) return 0;
+    return &(output->command());
+  }
+
+  virtual const Pose *getPoseCommand() const;
+  virtual const Twist *getTwistCommand() const;
+  virtual const Wrench *getWrenchCommand() const;
+  virtual const MotorCommand *getMotorCommand() const;
+
+  bool enabled(const CommandHandle *handle) const;
+  bool start(const CommandHandle *handle);
+  void stop(const CommandHandle *handle);
+
+  void disconnect(const CommandHandle *handle);
+
+private:
+//  typedef std::list< CommandHandlePtr > HandleList;
+//  std::map<const std::type_info *, HandleList> inputs_;
+//  std::map<const std::type_info *, HandleList> outputs_;
+  std::map<std::string, CommandHandlePtr> inputs_;
+  std::map<std::string, CommandHandlePtr> outputs_;
+  std::map<std::string, const CommandHandle *> enabled_;
+};
+
+} // namespace hector_quadrotor_controller
+
+#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/launch/controller.launch

@@ -0,0 +1,7 @@
+<launch>
+  <rosparam file="$(find hector_quadrotor_controller)/params/controller.yaml" />
+
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
+    controller/twist 
+     --shutdown-timeout 3"/>
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/package.xml

@@ -0,0 +1,42 @@
+<package>
+  <name>hector_quadrotor_controller</name>
+  <version>0.3.5</version>
+  <description>hector_quadrotor_controller provides libraries and a node for quadrotor control using <a href="http://wiki.ros.org/ros_control">ros_control</a>.</description>
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_quadrotor_controller</url>
+  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
+
+  <author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>hector_uav_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>hardware_interface</build_depend>
+  <build_depend>controller_interface</build_depend>
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>roscpp</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>hector_uav_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
+  <run_depend>hardware_interface</run_depend>
+  <run_depend>controller_interface</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+
+  <export>
+    <controller_interface plugin="${prefix}/plugin.xml"/>
+  </export>
+</package>

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/params/controller.yaml

@@ -0,0 +1,50 @@
+controller:
+    pose:
+        type: hector_quadrotor_controller/PoseController
+        xy:
+            k_p: 2.0
+            k_i: 0.0
+            k_d: 0.0
+            limit_output: 5.0
+        z:
+            k_p: 2.0
+            k_i: 0.0
+            k_d: 0.0
+            limit_output: 5.0
+        yaw:
+            k_p: 2.0
+            k_i: 0.0
+            k_d: 0.0
+            limit_output: 1.0
+    twist:
+        type: hector_quadrotor_controller/TwistController
+        linear/xy:
+            k_p: 5.0
+            k_i: 1.0
+            k_d: 0.0
+            limit_output: 10.0
+            time_constant: 0.05
+        linear/z:
+            k_p: 5.0
+            k_i: 1.0
+            k_d: 0.0
+            limit_output: 10.0
+            time_constant: 0.05
+        angular/xy:
+            k_p: 10.0
+            k_i: 5.0
+            k_d: 5.0
+            time_constant: 0.01
+        angular/z:
+            k_p: 5.0
+            k_i: 2.5
+            k_d: 0.0
+            limit_output: 3.0
+            time_constant: 0.1
+        limits:
+            load_factor: 1.5
+            force/z: 30.0
+            torque/xy: 10.0
+            torque/z: 1.0
+    motor:
+        type: hector_quadrotor_controller/MotorController

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/plugin.xml

@@ -0,0 +1,23 @@
+<class_libraries>
+  <library path="lib/libhector_quadrotor_pose_controller">
+    <class name="hector_quadrotor_controller/PoseController" type="hector_quadrotor_controller::PoseController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      The PoseController controls the quadrotor's pose and outputs velocity commands.
+    </description>
+    </class>
+  </library>
+  <library path="lib/libhector_quadrotor_twist_controller">
+    <class name="hector_quadrotor_controller/TwistController" type="hector_quadrotor_controller::TwistController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      The TwistController controls the quadrotor's twist (linear and angular velocity) by applying torque and thrust similar to a real quadrotor.
+    </description>
+    </class>
+  </library>
+  <library path="lib/libhector_quadrotor_motor_controller">
+    <class name="hector_quadrotor_controller/MotorController" type="hector_quadrotor_controller::MotorController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      The MotorController calculates the output voltages of the four motors from the commanded thrust and torque using a simple linearized model.
+    </description>
+    </class>
+  </library>
+</class_libraries>

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/motor_controller.cpp

@@ -0,0 +1,194 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#include <hector_quadrotor_controller/quadrotor_interface.h>
+#include <controller_interface/controller.h>
+
+#include <geometry_msgs/WrenchStamped.h>
+#include <std_srvs/Empty.h>
+
+#include <ros/subscriber.h>
+#include <ros/callback_queue.h>
+
+namespace hector_quadrotor_controller {
+
+using namespace controller_interface;
+
+class MotorController : public controller_interface::Controller<QuadrotorInterface>
+{
+public:
+  MotorController()
+    : node_handle_(0)
+  {}
+
+  ~MotorController()
+  {
+    if (node_handle_) {
+      node_handle_->shutdown();
+      delete node_handle_;
+      node_handle_ = 0;
+    }
+  }
+
+  bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
+  {
+    // get interface handles
+    wrench_input_  = interface->addInput<WrenchCommandHandle>("wrench");
+    motor_output_  = interface->addOutput<MotorCommandHandle>("motor");
+
+    // initialize NodeHandle
+    delete node_handle_;
+    node_handle_ = new ros::NodeHandle(root_nh);
+
+    // load parameters
+    controller_nh.getParam("force_per_voltage", parameters_.force_per_voltage = 0.559966216);
+    controller_nh.getParam("torque_per_voltage", parameters_.torque_per_voltage = 7.98598e-3);
+    controller_nh.getParam("lever", parameters_.lever = 0.275);
+    root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
+
+    // TODO: calculate these parameters from the quadrotor_propulsion parameters
+//    quadrotor_propulsion:
+//            k_t: 0.015336864714397
+//            k_m: -7.011631909766668e-5
+
+//            CT2s: -1.3077e-2
+//            CT1s: -2.5224e-4
+//            CT0s:  1.538190483976698e-5
+
+    return true;
+  }
+
+  void reset()
+  {
+    wrench_.wrench.force.x  = 0.0;
+    wrench_.wrench.force.y  = 0.0;
+    wrench_.wrench.force.z  = 0.0;
+    wrench_.wrench.torque.x = 0.0;
+    wrench_.wrench.torque.y = 0.0;
+    wrench_.wrench.torque.z = 0.0;
+
+    motor_.force.assign(4, 0.0);
+    motor_.torque.assign(4, 0.0);
+    motor_.frequency.clear();
+    motor_.voltage.assign(4, 0.0);
+  }
+
+  void wrenchCommandCallback(const geometry_msgs::WrenchStampedConstPtr& command)
+  {
+    wrench_ = *command;
+    if (wrench_.header.stamp.isZero()) wrench_.header.stamp = ros::Time::now();
+
+    // start controller if it not running
+    if (!isRunning()) this->startRequest(wrench_.header.stamp);
+  }
+
+  void starting(const ros::Time &time)
+  {
+    reset();
+    motor_output_->start();
+  }
+
+  void stopping(const ros::Time &time)
+  {
+    motor_output_->stop();
+  }
+
+  void update(const ros::Time& time, const ros::Duration& period)
+  {
+    // Get wrench command input
+    if (wrench_input_->connected() && wrench_input_->enabled()) {
+      wrench_.wrench = wrench_input_->getCommand();
+    }
+
+    // std::cout << "motor_controller:update" << std::endl; //added by zhangshuai@2018.12.18
+    // std::cout<<"wrench_.wrench.force.x:"<<wrench_.wrench.force.x<<std::endl;
+    // std::cout<<"wrench_.wrench.force.y:"<<wrench_.wrench.force.y<<std::endl;
+    // std::cout<<"wrench_.wrench.force.z:"<<wrench_.wrench.force.z<<std::endl;
+    // std::cout<<"wrench_.wrench.torque.x:"<<wrench_.wrench.torque.x<<std::endl;
+    // std::cout<<"wrench_.wrench.torque.y:"<<wrench_.wrench.torque.y<<std::endl;
+    // std::cout<<"wrench_.wrench.torque.z:"<<wrench_.wrench.torque.z<<std::endl;
+
+    // Update output
+    if (wrench_.wrench.force.z > 0.0) {
+
+      double nominal_thrust_per_motor = wrench_.wrench.force.z / 4.0;
+      motor_.force[0] =  nominal_thrust_per_motor - wrench_.wrench.torque.y / 2.0 / parameters_.lever;
+      motor_.force[1] =  nominal_thrust_per_motor - wrench_.wrench.torque.x / 2.0 / parameters_.lever;
+      motor_.force[2] =  nominal_thrust_per_motor + wrench_.wrench.torque.y / 2.0 / parameters_.lever;
+      motor_.force[3] =  nominal_thrust_per_motor + wrench_.wrench.torque.x / 2.0 / parameters_.lever;
+
+      double nominal_torque_per_motor = wrench_.wrench.torque.z / 4.0;
+      motor_.voltage[0] = motor_.force[0] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
+      motor_.voltage[1] = motor_.force[1] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
+      motor_.voltage[2] = motor_.force[2] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
+      motor_.voltage[3] = motor_.force[3] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
+
+      motor_.torque[0] = motor_.voltage[0] * parameters_.torque_per_voltage;
+      motor_.torque[1] = motor_.voltage[1] * parameters_.torque_per_voltage;
+      motor_.torque[2] = motor_.voltage[2] * parameters_.torque_per_voltage;
+      motor_.torque[3] = motor_.voltage[3] * parameters_.torque_per_voltage;
+
+      if (motor_.voltage[0] < 0.0) motor_.voltage[0] = 0.0;
+      if (motor_.voltage[1] < 0.0) motor_.voltage[1] = 0.0;
+      if (motor_.voltage[2] < 0.0) motor_.voltage[2] = 0.0;
+      if (motor_.voltage[3] < 0.0) motor_.voltage[3] = 0.0;
+
+    } else {
+      reset();
+    }
+
+    // set wrench output
+    motor_.header.stamp = time;
+    motor_.header.frame_id = "base_link";
+    motor_output_->setCommand(motor_);
+  }
+
+private:
+  WrenchCommandHandlePtr wrench_input_;
+  MotorCommandHandlePtr motor_output_;
+
+  ros::NodeHandle *node_handle_;
+  ros::Subscriber wrench_subscriber_;
+  ros::ServiceServer engage_service_server_;
+  ros::ServiceServer shutdown_service_server_;
+
+  geometry_msgs::WrenchStamped wrench_;
+  hector_uav_msgs::MotorCommand motor_;
+  std::string base_link_frame_;
+
+  struct {
+    double force_per_voltage;     // coefficient for linearized volts to force conversion for a single motor [N / V]
+    double torque_per_voltage;    // coefficient for linearized volts to force conversion for a single motor [Nm / V]
+    double lever;                 // the lever arm from origin to the motor axes (symmetry assumption) [m]
+  } parameters_;
+};
+
+} // namespace hector_quadrotor_controller
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::MotorController, controller_interface::ControllerBase)

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pid.cpp

@@ -0,0 +1,152 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#include <hector_quadrotor_controller/pid.h>
+#include <limits>
+
+namespace hector_quadrotor_controller {
+
+PID::state::state()
+  : p(std::numeric_limits<double>::quiet_NaN())
+  , i(0.0)
+  , d(std::numeric_limits<double>::quiet_NaN())
+  , input(std::numeric_limits<double>::quiet_NaN())
+  , dinput(0.0)
+  , dx(std::numeric_limits<double>::quiet_NaN())
+{
+}
+
+PID::parameters::parameters()
+  : enabled(true)
+  , time_constant(0.0)
+  , k_p(0.0)
+  , k_i(0.0)
+  , k_d(0.0)
+  , limit_i(std::numeric_limits<double>::quiet_NaN())
+  , limit_output(std::numeric_limits<double>::quiet_NaN())
+{
+}
+
+PID::PID()
+{}
+
+PID::PID(const parameters& params)
+  : parameters_(params)
+{}
+
+PID::~PID()
+{}
+
+void PID::init(const ros::NodeHandle &param_nh)
+{
+  param_nh.getParam("enabled", parameters_.enabled);
+  param_nh.getParam("k_p", parameters_.k_p);
+  param_nh.getParam("k_i", parameters_.k_i);
+  param_nh.getParam("k_d", parameters_.k_d);
+  param_nh.getParam("limit_i", parameters_.limit_i);
+  param_nh.getParam("limit_output", parameters_.limit_output);
+  param_nh.getParam("time_constant", parameters_.time_constant);
+}
+
+void PID::reset()
+{
+  state_ = state();
+}
+
+template <typename T> static inline T& checknan(T& value)
+{
+  if (std::isnan(value)) value = T();
+  return value;
+}
+
+double PID::update(double input, double x, double dx, const ros::Duration& dt)
+{
+  if (!parameters_.enabled) return 0.0;
+  double dt_sec = dt.toSec();
+
+  // low-pass filter input
+  if (std::isnan(state_.input)) state_.input = input;
+  if (dt_sec + parameters_.time_constant > 0.0) {
+    state_.dinput = (input - state_.input) / (dt_sec + parameters_.time_constant);
+    state_.input  = (dt_sec * input + parameters_.time_constant * state_.input) / (dt_sec + parameters_.time_constant);
+  }
+
+  return update(state_.input - x, dx, dt);
+}
+
+double PID::update(double error, double dx, const ros::Duration& dt)
+{
+  if (!parameters_.enabled) return 0.0;
+  if (std::isnan(error)) return 0.0;
+  double dt_sec = dt.toSec();
+
+  // integral error
+  state_.i += error * dt_sec;
+  if (parameters_.limit_i > 0.0)
+  {
+    if (state_.i >  parameters_.limit_i) state_.i =  parameters_.limit_i;
+    if (state_.i < -parameters_.limit_i) state_.i = -parameters_.limit_i;
+  }
+
+  // differential error
+  if (dt_sec > 0.0 && !std::isnan(state_.p) && !std::isnan(state_.dx)) {
+    state_.d = (error - state_.p) / dt_sec + state_.dx - dx;
+  } else {
+    state_.d = -dx;
+  }
+  state_.dx = dx;
+
+  // proportional error
+  state_.p = error;
+
+  // calculate output...
+  double output = parameters_.k_p * state_.p + parameters_.k_i * state_.i + parameters_.k_d * state_.d;
+  int antiwindup = 0;
+  if (parameters_.limit_output > 0.0)
+  {
+    if (output >  parameters_.limit_output) { output =  parameters_.limit_output; antiwindup =  1; }
+    if (output < -parameters_.limit_output) { output = -parameters_.limit_output; antiwindup = -1; }
+  }
+  if (antiwindup && (error * dt_sec * antiwindup > 0.0)) state_.i -= error * dt_sec;
+
+  checknan(output);
+  return output;
+}
+
+double PID::getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt)
+{
+  double dt_sec = dt.toSec();
+  filtered_error = checknan(filtered_error);
+  if (dt_sec + time_constant > 0.0) {
+    filtered_error = (time_constant * filtered_error + dt_sec * state_.p) / (dt_sec + time_constant);
+  }
+  return filtered_error;
+}
+
+} // namespace hector_quadrotor_controller
+

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pose_controller.cpp

@@ -0,0 +1,205 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#include <hector_quadrotor_controller/quadrotor_interface.h>
+#include <hector_quadrotor_controller/pid.h>
+
+#include <controller_interface/controller.h>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+
+#include <ros/subscriber.h>
+#include <ros/callback_queue.h>
+
+namespace hector_quadrotor_controller {
+
+using namespace controller_interface;
+
+class PoseController : public controller_interface::Controller<QuadrotorInterface>
+{
+public:
+  PoseController() {}
+
+  ~PoseController() {}
+
+  bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
+  {
+    // get interface handles
+    pose_         = interface->getPose();
+    twist_        = interface->getTwist();
+
+    pose_input_   = interface->addInput<PoseCommandHandle>("pose");
+    twist_input_  = interface->addInput<TwistCommandHandle>("pose/twist");
+    twist_limit_  = interface->addInput<TwistCommandHandle>("pose/twist_limit");
+    twist_output_ = interface->addOutput<TwistCommandHandle>("twist");
+
+    node_handle_ = root_nh;
+
+    // subscribe to commanded pose and velocity
+    pose_subscriber_ = node_handle_.subscribe<geometry_msgs::PoseStamped>("command/pose", 1, boost::bind(&PoseController::poseCommandCallback, this, _1));
+    twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&PoseController::twistCommandCallback, this, _1));
+
+    // initialize PID controllers
+    pid_.x.init(ros::NodeHandle(controller_nh, "xy"));
+    pid_.y.init(ros::NodeHandle(controller_nh, "xy"));
+    pid_.z.init(ros::NodeHandle(controller_nh, "z"));
+    pid_.yaw.init(ros::NodeHandle(controller_nh, "yaw"));
+
+    return true;
+  }
+
+  void reset()
+  {
+    pid_.x.reset();
+    pid_.y.reset();
+    pid_.z.reset();
+    pid_.yaw.reset();
+  }
+
+  void poseCommandCallback(const geometry_msgs::PoseStampedConstPtr& command)
+  {
+    pose_command_ = *command;
+    if (!(pose_input_->connected())) *pose_input_ = &(pose_command_.pose);
+    pose_input_->start();
+
+    ros::Time start_time = command->header.stamp;
+    if (start_time.isZero()) start_time = ros::Time::now();
+    if (!isRunning()) this->startRequest(start_time);
+  }
+
+  void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
+  {
+    twist_command_ = *command;
+    if (!(twist_input_->connected())) *twist_input_ = &(twist_command_.twist);
+    twist_input_->start();
+
+    ros::Time start_time = command->header.stamp;
+    if (start_time.isZero()) start_time = ros::Time::now();
+    if (!isRunning()) this->startRequest(start_time);
+  }
+
+  void starting(const ros::Time &time)
+  {
+    reset();
+    twist_output_->start();
+  }
+
+  void stopping(const ros::Time &time)
+  {
+    twist_output_->stop();
+  }
+
+  void update(const ros::Time& time, const ros::Duration& period)
+  {
+    // std::cout << "pose_controller:update" << std::endl; //added by zhangshuai@2018.12.18
+    Twist output;
+
+    // check command timeout
+    // TODO
+
+    // return if no pose command is available
+    if (pose_input_->enabled()) {
+      // control horizontal position
+      double error_n, error_w;
+      HorizontalPositionCommandHandle(*pose_input_).getError(*pose_, error_n, error_w);
+      output.linear.x = pid_.x.update(error_n, twist_->twist().linear.x, period);
+      output.linear.y = pid_.y.update(error_w, twist_->twist().linear.y, period);
+
+      // control height
+      output.linear.z = pid_.z.update(HeightCommandHandle(*pose_input_).getError(*pose_), twist_->twist().linear.z, period);
+
+      // control yaw angle
+      output.angular.z = pid_.yaw.update(HeadingCommandHandle(*pose_input_).getError(*pose_), twist_->twist().angular.z, period);
+    }
+
+    // add twist command if available
+    if (twist_input_->enabled())
+    {
+      output.linear.x  += twist_input_->getCommand().linear.x;
+      output.linear.y  += twist_input_->getCommand().linear.y;
+      output.linear.z  += twist_input_->getCommand().linear.z;
+      output.angular.x += twist_input_->getCommand().angular.x;
+      output.angular.y += twist_input_->getCommand().angular.y;
+      output.angular.z += twist_input_->getCommand().angular.z;
+    }
+
+    // limit twist
+    if (twist_limit_->enabled())
+    {
+      double linear_xy = sqrt(output.linear.x*output.linear.x + output.linear.y*output.linear.y);
+      double limit_linear_xy  = std::max(twist_limit_->get()->linear.x, twist_limit_->get()->linear.y);
+      if (limit_linear_xy > 0.0 && linear_xy > limit_linear_xy) {
+        output.linear.x *= limit_linear_xy / linear_xy;
+        output.linear.y *= limit_linear_xy / linear_xy;
+      }
+      if (twist_limit_->get()->linear.z > 0.0 && fabs(output.linear.z) > twist_limit_->get()->linear.z) {
+        output.linear.z *= twist_limit_->get()->linear.z / fabs(output.linear.z);
+      }
+      double angular_xy = sqrt(output.angular.x*output.angular.x + output.angular.y*output.angular.y);
+      double limit_angular_xy  = std::max(twist_limit_->get()->angular.x, twist_limit_->get()->angular.y);
+      if (limit_angular_xy > 0.0 && angular_xy > limit_angular_xy) {
+        output.angular.x *= limit_angular_xy / angular_xy;
+        output.angular.y *= limit_angular_xy / angular_xy;
+      }
+      if (twist_limit_->get()->angular.z > 0.0 && fabs(output.angular.z) > twist_limit_->get()->angular.z) {
+        output.angular.z *= twist_limit_->get()->angular.z / fabs(output.angular.z);
+      }
+    }
+
+    // set twist output
+    twist_output_->setCommand(output);
+  }
+
+private:
+  PoseHandlePtr pose_;
+  PoseCommandHandlePtr pose_input_;
+  TwistHandlePtr twist_;
+  TwistCommandHandlePtr twist_input_;
+  TwistCommandHandlePtr twist_limit_;
+  TwistCommandHandlePtr twist_output_;
+
+  geometry_msgs::PoseStamped pose_command_;
+  geometry_msgs::TwistStamped twist_command_;
+
+  ros::NodeHandle node_handle_;
+  ros::Subscriber pose_subscriber_;
+  ros::Subscriber twist_subscriber_;
+
+  struct {
+    PID x;
+    PID y;
+    PID z;
+    PID yaw;
+  } pid_;
+};
+
+} // namespace hector_quadrotor_controller
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::PoseController, controller_interface::ControllerBase)

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/quadrotor_interface.cpp

@@ -0,0 +1,195 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#include <hector_quadrotor_controller/quadrotor_interface.h>
+
+#include <cmath>
+
+namespace hector_quadrotor_controller {
+
+QuadrotorInterface::QuadrotorInterface()
+{}
+
+QuadrotorInterface::~QuadrotorInterface()
+{}
+
+bool QuadrotorInterface::enabled(const CommandHandle *handle) const
+{
+  if (!handle || !handle->connected()) return false;
+  std::string resource = handle->getName();
+  return enabled_.count(resource) > 0;
+}
+
+bool QuadrotorInterface::start(const CommandHandle *handle)
+{
+  if (!handle || !handle->connected()) return false;
+  if (enabled(handle)) return true;
+  std::string resource = handle->getName();
+  enabled_[resource] = handle;
+  ROS_DEBUG_NAMED("quadrotor_interface", "Enabled %s control", resource.c_str());
+  return true;
+}
+
+void QuadrotorInterface::stop(const CommandHandle *handle)
+{
+  if (!handle) return;
+  if (!enabled(handle)) return;
+  std::string resource = handle->getName();
+  std::map<std::string, const CommandHandle *>::iterator it = enabled_.lower_bound(resource);
+  if (it != enabled_.end() && it->second == handle) enabled_.erase(it);
+  ROS_DEBUG_NAMED("quadrotor_interface", "Disabled %s control", resource.c_str());
+}
+
+void QuadrotorInterface::disconnect(const CommandHandle *handle)
+{
+  if (!handle) return;
+  std::string resource = handle->getName();
+  if (inputs_.count(resource)) {
+    const CommandHandlePtr& input = inputs_.at(resource);
+    if (input.get() != handle) input->reset();
+  }
+  if (outputs_.count(resource)) {
+    const CommandHandlePtr& output = outputs_.at(resource);
+    if (output.get() != handle) output->reset();
+  }
+}
+
+const Pose *QuadrotorInterface::getPoseCommand()          const { return getCommand<PoseCommandHandle>("pose"); }
+const Twist *QuadrotorInterface::getTwistCommand()        const { return getCommand<TwistCommandHandle>("twist"); }
+const Wrench *QuadrotorInterface::getWrenchCommand()      const { return getCommand<WrenchCommandHandle>("wrench"); }
+const MotorCommand *QuadrotorInterface::getMotorCommand() const { return getCommand<MotorCommandHandle>("motor"); }
+
+void PoseHandle::getEulerRPY(double &roll, double &pitch, double &yaw) const
+{
+  const Quaternion::_w_type& w = pose().orientation.w;
+  const Quaternion::_x_type& x = pose().orientation.x;
+  const Quaternion::_y_type& y = pose().orientation.y;
+  const Quaternion::_z_type& z = pose().orientation.z;
+  roll  =  atan2(2.*y*z + 2.*w*x, z*z - y*y - x*x + w*w);
+  pitch = -asin(2.*x*z - 2.*w*y);
+  yaw   =  atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
+}
+
+double PoseHandle::getYaw() const
+{
+  const Quaternion::_w_type& w = pose().orientation.w;
+  const Quaternion::_x_type& x = pose().orientation.x;
+  const Quaternion::_y_type& y = pose().orientation.y;
+  const Quaternion::_z_type& z = pose().orientation.z;
+  return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
+}
+
+Vector3 PoseHandle::toBody(const Vector3& nav) const
+{
+  const Quaternion::_w_type& w = pose().orientation.w;
+  const Quaternion::_x_type& x = pose().orientation.x;
+  const Quaternion::_y_type& y = pose().orientation.y;
+  const Quaternion::_z_type& z = pose().orientation.z;
+  Vector3 body;
+  body.x = (w*w+x*x-y*y-z*z) * nav.x + (2.*x*y + 2.*w*z) * nav.y + (2.*x*z - 2.*w*y) * nav.z;
+  body.y = (2.*x*y - 2.*w*z) * nav.x + (w*w-x*x+y*y-z*z) * nav.y + (2.*y*z + 2.*w*x) * nav.z;
+  body.z = (2.*x*z + 2.*w*y) * nav.x + (2.*y*z - 2.*w*x) * nav.y + (w*w-x*x-y*y+z*z) * nav.z;
+  return body;
+}
+
+Vector3 PoseHandle::fromBody(const Vector3& body) const
+{
+  const Quaternion::_w_type& w = pose().orientation.w;
+  const Quaternion::_x_type& x = pose().orientation.x;
+  const Quaternion::_y_type& y = pose().orientation.y;
+  const Quaternion::_z_type& z = pose().orientation.z;
+  Vector3 nav;
+  nav.x = (w*w+x*x-y*y-z*z) * body.x + (2.*x*y - 2.*w*z) * body.y + (2.*x*z + 2.*w*y) * body.z;
+  nav.y = (2.*x*y + 2.*w*z) * body.x + (w*w-x*x+y*y-z*z) * body.y + (2.*y*z - 2.*w*x) * body.z;
+  nav.z = (2.*x*z - 2.*w*y) * body.x + (2.*y*z + 2.*w*x) * body.y + (w*w-x*x-y*y+z*z) * body.z;
+  return nav;
+}
+
+void HorizontalPositionCommandHandle::getError(const PoseHandle &pose, double &x, double &y) const
+{
+  getCommand(x, y);
+  x -= pose.get()->position.x;
+  y -= pose.get()->position.y;
+}
+
+double HeightCommandHandle::getError(const PoseHandle &pose) const
+{
+  return getCommand() - pose.get()->position.z;
+}
+
+void HeadingCommandHandle::setCommand(double command)
+{
+  if (get()) {
+    get()->x = 0.0;
+    get()->y = 0.0;
+    get()->z = sin(command / 2.);
+    get()->w = cos(command / 2.);
+  }
+
+  if (scalar_) {
+    *scalar_ = command;
+  }
+}
+
+double HeadingCommandHandle::getCommand() const {
+  if (scalar_) return *scalar_;
+  const Quaternion::_w_type& w = get()->w;
+  const Quaternion::_x_type& x = get()->x;
+  const Quaternion::_y_type& y = get()->y;
+  const Quaternion::_z_type& z = get()->z;
+  return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
+}
+
+bool HeadingCommandHandle::update(Pose& command) const {
+  if (get()) {
+    command.orientation = *get();
+    return true;
+  }
+  if (scalar_) {
+    command.orientation.x = 0.0;
+    command.orientation.y = 0.0;
+    command.orientation.z = sin(*scalar_ / 2.);
+    command.orientation.x = cos(*scalar_ / 2.);
+    return true;
+  }
+  return false;
+}
+
+double HeadingCommandHandle::getError(const PoseHandle &pose) const {
+  static const double M_2PI = 2.0 * M_PI;
+  double error = getCommand() - pose.getYaw() + M_PI;
+  error -= floor(error / M_2PI) * M_2PI;
+  return error - M_PI;
+}
+
+bool CommandHandle::enabled()    { return interface_->enabled(this); }
+bool CommandHandle::start()      { return interface_->start(this); }
+void CommandHandle::stop()       { interface_->stop(this); }
+void CommandHandle::disconnect() { interface_->disconnect(this); }
+
+} // namespace hector_quadrotor_controller

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller (复件).cpp

@@ -0,0 +1,328 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#include <hector_quadrotor_controller/quadrotor_interface.h>
+#include <hector_quadrotor_controller/pid.h>
+
+#include <controller_interface/controller.h>
+
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <std_srvs/Empty.h>
+
+#include <ros/subscriber.h>
+#include <ros/callback_queue.h>
+
+#include <boost/thread.hpp>
+
+#include <limits>
+
+namespace hector_quadrotor_controller {
+
+using namespace controller_interface;
+
+class TwistController : public controller_interface::Controller<QuadrotorInterface>
+{
+public:
+  TwistController()
+  {}
+
+  ~TwistController()
+  {}
+
+  bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
+  {
+    // get interface handles
+    pose_          = interface->getPose();
+    twist_         = interface->getTwist();
+    acceleration_  = interface->getAcceleration();
+    twist_input_   = interface->addInput<TwistCommandHandle>("twist");
+    wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
+    node_handle_ = root_nh;
+
+    // subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist)
+    twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
+    cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("/fixedwing/cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
+
+    // engage/shutdown service servers
+    engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
+    shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
+
+    // initialize PID controllers
+    pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
+    pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
+    pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
+    pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
+    pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
+    pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
+
+    // load other parameters
+    controller_nh.getParam("auto_engage", auto_engage_ = true);
+    controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
+    controller_nh.getParam("limits/force/z", limits_.force.z);
+    controller_nh.getParam("limits/torque/xy", limits_.torque.x);
+    controller_nh.getParam("limits/torque/xy", limits_.torque.y);
+    controller_nh.getParam("limits/torque/z", limits_.torque.z);
+    root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
+
+    // get mass and inertia from QuadrotorInterface
+    interface->getMassAndInertia(mass_, inertia_);
+
+    command_given_in_stabilized_frame_ = false;
+
+    return true;
+  }
+
+  void reset()
+  {
+    pid_.linear.x.reset();
+    pid_.linear.y.reset();
+    pid_.linear.z.reset();
+    pid_.angular.x.reset();
+    pid_.angular.y.reset();
+    pid_.angular.z.reset();
+
+    wrench_.wrench.force.x  = 0.0;
+    wrench_.wrench.force.y  = 0.0;
+    wrench_.wrench.force.z  = 0.0;
+    wrench_.wrench.torque.x = 0.0;
+    wrench_.wrench.torque.y = 0.0;
+    wrench_.wrench.torque.z = 0.0;
+
+    linear_z_control_error_ = 0.0;
+    motors_running_ = false;
+  }
+
+  void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);
+
+    command_ = *command;
+    if (command_.header.stamp.isZero()) command_.header.stamp = ros::Time::now();
+    command_given_in_stabilized_frame_ = false;
+
+    // start controller if it not running
+    if (!isRunning()) this->startRequest(command_.header.stamp);
+  }
+
+  void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr& command)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);
+
+    command_.twist = *command;
+    command_.header.stamp = ros::Time::now();
+    command_given_in_stabilized_frame_ = true;
+
+    // start controller if it not running
+    if (!isRunning()) this->startRequest(command_.header.stamp);
+  }
+
+  bool engageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);
+
+    ROS_INFO_NAMED("twist_controller", "Engaging motors!");
+    motors_running_ = true;
+    return true;
+  }
+
+  bool shutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);
+
+    ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
+    motors_running_ = false;
+    return true;
+  }
+
+  void starting(const ros::Time &time)
+  {
+    reset();
+    wrench_output_->start();
+  }
+
+  void stopping(const ros::Time &time)
+  {
+    wrench_output_->stop();
+  }
+
+  void update(const ros::Time& time, const ros::Duration& period)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);
+
+    // Get twist command input
+    if (twist_input_->connected() && twist_input_->enabled()) {
+      command_.twist = twist_input_->getCommand();
+      command_given_in_stabilized_frame_ = false;
+    }
+
+    // Get current state and command
+    Twist command = command_.twist;
+    Twist twist = twist_->twist();
+    Twist twist_body;
+    twist_body.linear =  pose_->toBody(twist.linear);
+    twist_body.angular = pose_->toBody(twist.angular);
+
+    // Transform to world coordinates if necessary (yaw only)
+    if (command_given_in_stabilized_frame_) {
+      double yaw = pose_->getYaw();
+      Twist transformed = command;
+      transformed.linear.x  = cos(yaw) * command.linear.x  - sin(yaw) * command.linear.y;
+      transformed.linear.y  = sin(yaw) * command.linear.x  + cos(yaw) * command.linear.y;
+      transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
+      transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
+      command = transformed;
+    }
+
+    // Get gravity and load factor
+    const double gravity = 9.8065;
+    double load_factor = 1. / (  pose_->pose().orientation.w * pose_->pose().orientation.w
+                                 - pose_->pose().orientation.x * pose_->pose().orientation.x
+                                 - pose_->pose().orientation.y * pose_->pose().orientation.y
+                                 + pose_->pose().orientation.z * pose_->pose().orientation.z );
+    // Note: load_factor could be NaN or Inf...?
+    if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit)) load_factor = load_factor_limit;
+
+    // Auto engage/shutdown
+    if (auto_engage_) {
+      if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0) {
+        motors_running_ = true;
+        ROS_INFO_NAMED("twist_controller", "Engaging motors!");
+      } else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */) {
+        double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
+        if (linear_z_control_error_ > 0.0) linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown
+        if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit) {
+          motors_running_ = false;
+          ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
+        } else {
+          ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
+        }
+      } else {
+        linear_z_control_error_ = 0.0;
+      }
+
+      // flip over?
+      if (motors_running_ && load_factor < 0.0) {
+        motors_running_ = false;
+        ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
+      }
+    }
+
+    // Update output
+    if (motors_running_) {
+      Vector3 acceleration_command;
+      acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
+      acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
+      acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
+      Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
+
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear:               [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular:         [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear:       [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular:      [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration:               [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body:  [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
+
+      wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
+      wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update( acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
+      wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update( command.angular.z, twist.angular.z, 0.0, period);
+      wrench_.wrench.force.x  = 0.0;
+      wrench_.wrench.force.y  = 0.0;
+      wrench_.wrench.force.z  = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
+
+      if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z) wrench_.wrench.force.z = limits_.force.z;
+      if (wrench_.wrench.force.z <= std::numeric_limits<double>::min()) wrench_.wrench.force.z = std::numeric_limits<double>::min();
+      if (limits_.torque.x > 0.0) {
+        if (wrench_.wrench.torque.x >  limits_.torque.x) wrench_.wrench.torque.x =  limits_.torque.x;
+        if (wrench_.wrench.torque.x < -limits_.torque.x) wrench_.wrench.torque.x = -limits_.torque.x;
+      }
+      if (limits_.torque.y > 0.0) {
+        if (wrench_.wrench.torque.y >  limits_.torque.y) wrench_.wrench.torque.y =  limits_.torque.y;
+        if (wrench_.wrench.torque.y < -limits_.torque.y) wrench_.wrench.torque.y = -limits_.torque.y;
+      }
+      if (limits_.torque.z > 0.0) {
+        if (wrench_.wrench.torque.z >  limits_.torque.z) wrench_.wrench.torque.z =  limits_.torque.z;
+        if (wrench_.wrench.torque.z < -limits_.torque.z) wrench_.wrench.torque.z = -limits_.torque.z;
+      }
+
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force:       [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque:      [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
+
+    } else {
+      reset();
+    }
+
+    // set wrench output
+    wrench_.header.stamp = time;
+    wrench_.header.frame_id = base_link_frame_;
+    wrench_output_->setCommand(wrench_.wrench);
+  }
+
+private:
+  PoseHandlePtr pose_;
+  TwistHandlePtr twist_;
+  AccelerationHandlePtr acceleration_;
+  TwistCommandHandlePtr twist_input_;
+  WrenchCommandHandlePtr wrench_output_;
+
+  ros::NodeHandle node_handle_;
+  ros::Subscriber twist_subscriber_;
+  ros::Subscriber cmd_vel_subscriber_;
+  ros::ServiceServer engage_service_server_;
+  ros::ServiceServer shutdown_service_server_;
+
+  geometry_msgs::TwistStamped command_;
+  geometry_msgs::WrenchStamped wrench_;
+  bool command_given_in_stabilized_frame_;
+  std::string base_link_frame_;
+
+  struct {
+    struct {
+      PID x;
+      PID y;
+      PID z;
+    } linear, angular;
+  } pid_;
+
+  geometry_msgs::Wrench limits_;
+  bool auto_engage_;
+  double load_factor_limit;
+  double mass_;
+  double inertia_[3];
+
+  bool motors_running_;
+  double linear_z_control_error_;
+  boost::mutex command_mutex_;
+
+};
+
+} // namespace hector_quadrotor_controller
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)

Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller.cpp

@@ -0,0 +1,408 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#include <hector_quadrotor_controller/quadrotor_interface.h>
+#include <hector_quadrotor_controller/pid.h>
+
+#include <controller_interface/controller.h>
+
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <std_srvs/Empty.h>
+
+#include <ros/subscriber.h>
+#include <ros/callback_queue.h>
+
+#include <boost/thread.hpp>
+
+#include <limits>
+
+// #ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.19
+// #define _COUNT_TIME_HH_
+
+// #ifndef _WIN32
+// #include <sys/time.h>
+// #endif
+
+// #ifndef USE_COUNT_TIME
+// #define USE_COUNT_TIME
+// #endif
+
+// #endif
+
+namespace hector_quadrotor_controller
+{
+
+using namespace controller_interface;
+
+class TwistController : public controller_interface::Controller<QuadrotorInterface>
+{
+public:
+  TwistController()
+  {
+  }
+
+  ~TwistController()
+  {
+  }
+
+  bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
+  {
+    // get interface handles
+    pose_ = interface->getPose();
+    twist_ = interface->getTwist();
+    acceleration_ = interface->getAcceleration();
+    twist_input_ = interface->addInput<TwistCommandHandle>("twist");
+    wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
+    node_handle_ = root_nh;
+
+    // subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist)
+    twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
+    cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
+
+    // engage/shutdown service servers
+    engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
+    shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
+
+    // initialize PID controllers
+    pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
+    pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
+    pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
+    pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
+    pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
+    pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
+
+    // load other parameters
+    controller_nh.getParam("auto_engage", auto_engage_ = true);
+    controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
+    controller_nh.getParam("limits/force/z", limits_.force.z);
+    controller_nh.getParam("limits/torque/xy", limits_.torque.x);
+    controller_nh.getParam("limits/torque/xy", limits_.torque.y);
+    controller_nh.getParam("limits/torque/z", limits_.torque.z);
+    root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
+
+    // get mass and inertia from QuadrotorInterface
+    interface->getMassAndInertia(mass_, inertia_);
+
+    command_given_in_stabilized_frame_ = false;
+
+    return true;
+  }
+
+  void reset()
+  {
+    pid_.linear.x.reset();
+    pid_.linear.y.reset();
+    pid_.linear.z.reset();
+    pid_.angular.x.reset();
+    pid_.angular.y.reset();
+    pid_.angular.z.reset();
+
+    wrench_.wrench.force.x = 0.0;
+    wrench_.wrench.force.y = 0.0;
+    wrench_.wrench.force.z = 0.0;
+    wrench_.wrench.torque.x = 0.0;
+    wrench_.wrench.torque.y = 0.0;
+    wrench_.wrench.torque.z = 0.0;
+
+    linear_z_control_error_ = 0.0;
+    motors_running_ = false;
+  }
+
+  void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr &command)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);                 
+
+    command_ = *command;
+    if (command_.header.stamp.isZero())
+      command_.header.stamp = ros::Time::now();
+    command_given_in_stabilized_frame_ = false;
+
+    // start controller if it not running
+    if (!isRunning())
+      this->startRequest(command_.header.stamp);
+  }
+
+  void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr &command)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);               
+
+    command_.twist = *command;
+    command_.header.stamp = ros::Time::now();
+    command_given_in_stabilized_frame_ = true;
+
+    // start controller if it not running
+    if (!isRunning())
+      this->startRequest(command_.header.stamp);
+  }
+
+  bool engageCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);               
+
+    ROS_INFO_NAMED("twist_controller", "Engaging motors!");
+    motors_running_ = true;
+    return true;
+  }
+
+  bool shutdownCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
+  {
+    boost::mutex::scoped_lock lock(command_mutex_);                  
+
+    ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
+    motors_running_ = false;
+    return true;
+  }
+
+  void starting(const ros::Time &time)
+  {
+    reset();
+    wrench_output_->start();
+  }
+
+  void stopping(const ros::Time &time)
+  {
+    wrench_output_->stop();
+  }
+
+// int i=0;   //added by zhangshuai@2018.12.18
+#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
+  double twist_controller_update_time = 0.0;
+  int num = 0;
+// double UpdateTime = 0;
+// double ProcessTime = 0;
+// double publishModelPoses_time = 0;
+#endif
+
+  void update(const ros::Time &time, const ros::Duration &period)
+  {
+#ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19
+    struct timeval tv;
+    double start_time, cur_time, tmp_time;
+    gettimeofday(&tv, NULL);
+    start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
+#endif
+
+    // std::cout<<"update:"<<i<<std::endl;   //added by zhangshuai@2018.12.18
+    // i++;    //added by zhangshuai@2018.12.18
+
+    boost::mutex::scoped_lock lock(command_mutex_);                    
+
+    // Get twist command input
+    if (twist_input_->connected() && twist_input_->enabled())
+    {
+      command_.twist = twist_input_->getCommand();
+      command_given_in_stabilized_frame_ = false;
+    }
+
+    // Get current state and command
+    Twist command = command_.twist;
+    Twist twist = twist_->twist();
+    Twist twist_body;
+    twist_body.linear = pose_->toBody(twist.linear);
+    twist_body.angular = pose_->toBody(twist.angular);
+
+    // Transform to world coordinates if necessary (yaw only)
+    if (command_given_in_stabilized_frame_)
+    {
+      double yaw = pose_->getYaw();
+      Twist transformed = command;
+      transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y;
+      transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y;
+      transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
+      transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
+      command = transformed;
+    }
+
+    // Get gravity and load factor
+    const double gravity = 9.8065;
+    double load_factor = 1. / (pose_->pose().orientation.w * pose_->pose().orientation.w - pose_->pose().orientation.x * pose_->pose().orientation.x - pose_->pose().orientation.y * pose_->pose().orientation.y + pose_->pose().orientation.z * pose_->pose().orientation.z);
+    // Note: load_factor could be NaN or Inf...?
+    if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit))
+      load_factor = load_factor_limit;
+
+    // Auto engage/shutdown
+    if (auto_engage_)
+    {
+      if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0)
+      {
+        motors_running_ = true;
+        ROS_INFO_NAMED("twist_controller", "Engaging motors!");
+      }
+      else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */)
+      {
+        double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
+        if (linear_z_control_error_ > 0.0)
+          linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown
+        if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit)
+        {
+          motors_running_ = false;
+          ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
+        }
+        else
+        {
+          ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
+        }
+      }
+      else
+      {
+        linear_z_control_error_ = 0.0;
+      }
+
+      // flip over?
+      if (motors_running_ && load_factor < 0.0)
+      {
+        motors_running_ = false;
+        ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
+      }
+    }
+
+    // Update output
+    if (motors_running_)
+    {
+      Vector3 acceleration_command;
+      acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
+      acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
+      acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
+      Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
+
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear:               [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular:         [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear:       [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular:      [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration:               [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body:  [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
+
+      wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
+      wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update(acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
+      wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update(command.angular.z, twist.angular.z, 0.0, period);
+      wrench_.wrench.force.x = 0.0;
+      wrench_.wrench.force.y = 0.0;
+      wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
+
+      if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z)
+        wrench_.wrench.force.z = limits_.force.z;
+      if (wrench_.wrench.force.z <= std::numeric_limits<double>::min())
+        wrench_.wrench.force.z = std::numeric_limits<double>::min();
+      if (limits_.torque.x > 0.0)
+      {
+        if (wrench_.wrench.torque.x > limits_.torque.x)
+          wrench_.wrench.torque.x = limits_.torque.x;
+        if (wrench_.wrench.torque.x < -limits_.torque.x)
+          wrench_.wrench.torque.x = -limits_.torque.x;
+      }
+      if (limits_.torque.y > 0.0)
+      {
+        if (wrench_.wrench.torque.y > limits_.torque.y)
+          wrench_.wrench.torque.y = limits_.torque.y;
+        if (wrench_.wrench.torque.y < -limits_.torque.y)
+          wrench_.wrench.torque.y = -limits_.torque.y;
+      }
+      if (limits_.torque.z > 0.0)
+      {
+        if (wrench_.wrench.torque.z > limits_.torque.z)
+          wrench_.wrench.torque.z = limits_.torque.z;
+        if (wrench_.wrench.torque.z < -limits_.torque.z)
+          wrench_.wrench.torque.z = -limits_.torque.z;
+      }
+
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force:       [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
+      ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque:      [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
+    }
+    else
+    {
+      reset();
+    }
+
+    // set wrench output
+    wrench_.header.stamp = time;
+    wrench_.header.frame_id = base_link_frame_;
+    // std::cout << "twist_controller:update" << std::endl; //added by zhangshuai@2018.12.18
+    // std::cout<<"wrench_.wrench.force.x:"<<wrench_.wrench.force.x<<std::endl;
+    // std::cout<<"wrench_.wrench.force.y:"<<wrench_.wrench.force.y<<std::endl;
+    // std::cout<<"wrench_.wrench.force.z:"<<wrench_.wrench.force.z<<std::endl;
+    // std::cout<<"wrench_.wrench.torque.x:"<<wrench_.wrench.torque.x<<std::endl;
+    // std::cout<<"wrench_.wrench.torque.y:"<<wrench_.wrench.torque.y<<std::endl;
+    // std::cout<<"wrench_.wrench.torque.z:"<<wrench_.wrench.torque.z<<std::endl;
+    wrench_output_->setCommand(wrench_.wrench);
+
+#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
+    gettimeofday(&tv, NULL);
+    num++;
+    twist_controller_update_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
+    if (num == 9999)
+    {
+      std::cout << "twist_controller_update_time: " << twist_controller_update_time << "num:" << num << std::endl;
+    }
+#endif
+  }
+
+private:
+  PoseHandlePtr pose_;
+  TwistHandlePtr twist_;
+  AccelerationHandlePtr acceleration_;
+  TwistCommandHandlePtr twist_input_;
+  WrenchCommandHandlePtr wrench_output_;
+
+  ros::NodeHandle node_handle_;
+  ros::Subscriber twist_subscriber_;
+  ros::Subscriber cmd_vel_subscriber_;
+  ros::ServiceServer engage_service_server_;
+  ros::ServiceServer shutdown_service_server_;
+
+  geometry_msgs::TwistStamped command_;
+  geometry_msgs::WrenchStamped wrench_;
+  bool command_given_in_stabilized_frame_;
+  std::string base_link_frame_;
+
+  struct
+  {
+    struct
+    {
+      PID x;
+      PID y;
+      PID z;
+    } linear, angular;
+  } pid_;
+
+  geometry_msgs::Wrench limits_;
+  bool auto_engage_;
+  double load_factor_limit;
+  double mass_;
+  double inertia_[3];
+
+  bool motors_running_;
+  double linear_z_control_error_;
+  boost::mutex command_mutex_;
+};
+
+} // namespace hector_quadrotor_controller
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)

Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst

@@ -0,0 +1,20 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_quadrotor_controller_gazebo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.5 (2015-03-28)
+------------------
+
+0.3.4 (2015-02-22)
+------------------
+
+0.3.3 (2014-09-01)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-12-26)
+------------------
+* New controller implementation using ros_control
+* Contributors: Johannes Meyer

Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt

@@ -0,0 +1,122 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hector_quadrotor_controller_gazebo)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  gazebo_ros_control
+  hector_quadrotor_controller
+)
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# Depend on system install of Gazebo
+find_package(gazebo REQUIRED)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+link_directories(${GAZEBO_LIBRARY_DIRS})
+include_directories(${GAZEBO_INCLUDE_DIRS})
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES hector_quadrotor_controller_gazebo
+  CATKIN_DEPENDS gazebo_ros_control hector_quadrotor_controller
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+add_library(hector_quadrotor_controller_gazebo
+  src/quadrotor_hardware_gazebo.cpp
+)
+target_link_libraries(hector_quadrotor_controller_gazebo
+  ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS hector_quadrotor_controller_gazebo
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  quadrotor_controller_gazebo.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_hector_quadrotor_controller_gazebo.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller/quadrotor_hardware_gazebo.h

@@ -0,0 +1,109 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
+#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
+
+#include <gazebo_ros_control/robot_hw_sim.h>
+#include <hector_quadrotor_controller/quadrotor_interface.h>
+
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
+#include <hector_uav_msgs/MotorStatus.h>
+
+#include <ros/node_handle.h>
+#include <ros/callback_queue.h>
+
+namespace hector_quadrotor_controller_gazebo {
+
+using namespace hector_quadrotor_controller;
+using namespace hardware_interface;
+using namespace gazebo_ros_control;
+
+class QuadrotorHardwareSim : public RobotHWSim, public QuadrotorInterface
+{
+public:
+  QuadrotorHardwareSim();
+  virtual ~QuadrotorHardwareSim();
+
+  virtual const ros::Time &getTimestamp() { return header_.stamp; }
+
+  virtual PoseHandlePtr getPose()                 { return PoseHandlePtr(new PoseHandle(this, &pose_)); }
+  virtual TwistHandlePtr getTwist()               { return TwistHandlePtr(new TwistHandle(this, &twist_)); }
+  virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(new AccelerationHandle(this, &acceleration_)); }
+  virtual ImuHandlePtr getSensorImu()             { return ImuHandlePtr(new ImuHandle(this, &imu_)); }
+  virtual MotorStatusHandlePtr getMotorStatus()   { return MotorStatusHandlePtr(new MotorStatusHandle(this, &motor_status_)); }
+
+  virtual bool getMassAndInertia(double &mass, double inertia[3]);
+
+  virtual bool initSim(
+      const std::string& robot_namespace,
+      ros::NodeHandle model_nh,
+      gazebo::physics::ModelPtr parent_model,
+      const urdf::Model *const urdf_model,
+      std::vector<transmission_interface::TransmissionInfo> transmissions);
+
+  virtual void readSim(ros::Time time, ros::Duration period);
+
+  virtual void writeSim(ros::Time time, ros::Duration period);
+
+private:
+  void stateCallback(const nav_msgs::OdometryConstPtr &state);
+  void imuCallback(const sensor_msgs::ImuConstPtr &imu);
+  void motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status);
+
+protected:
+  std_msgs::Header header_;
+  Pose pose_;
+  Twist twist_;
+  Vector3 acceleration_;
+  Imu imu_;
+  MotorStatus motor_status_;
+  std::string base_link_frame_, world_frame_;
+
+  WrenchCommandHandlePtr wrench_output_;
+  MotorCommandHandlePtr motor_output_;
+
+  gazebo::physics::ModelPtr model_;
+  gazebo::physics::LinkPtr link_;
+  gazebo::physics::PhysicsEnginePtr physics_;
+
+  gazebo::math::Pose gz_pose_;
+  gazebo::math::Vector3 gz_velocity_, gz_acceleration_, gz_angular_velocity_;
+
+  ros::CallbackQueue callback_queue_;
+  ros::Subscriber subscriber_state_;
+  ros::Subscriber subscriber_imu_;
+  ros::Subscriber subscriber_motor_status_;
+  ros::Publisher publisher_wrench_command_;
+  ros::Publisher publisher_motor_command_;
+};
+
+} // namespace hector_quadrotor_controller_gazebo
+
+#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H

Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package>
+  <name>hector_quadrotor_controller_gazebo</name>
+  <version>0.3.5</version>
+  <description>The hector_quadrotor_controller_gazebo package implements the ros_control RobotHWSim interface for the quadrotor controller in package hector_quadrotor_controller.</description>
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_quadrotor_controller_gazebo</url>
+  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
+
+  <author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend version_gte="2.3.4">gazebo_ros_control</build_depend>
+  <build_depend>hector_quadrotor_controller</build_depend>
+  <run_depend version_gte="2.3.4">gazebo_ros_control</run_depend>
+  <run_depend>hector_quadrotor_controller</run_depend>
+
+  <export>
+    <gazebo_ros_control plugin="${prefix}/quadrotor_controller_gazebo.xml"/>
+  </export>
+</package>

Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml

@@ -0,0 +1,7 @@
+<library path="lib/libhector_quadrotor_controller_gazebo">
+  <class name="hector_quadrotor_controller_gazebo/QuadrotorHardwareSim" type="hector_quadrotor_controller_gazebo::QuadrotorHardwareSim" base_class_type="gazebo_ros_control::RobotHWSim">
+  <description>
+    This class represents the quadrotor hardware in Gazebo for the gazebo_ros_control plugin.
+  </description>
+  </class>
+</library>

Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp

@@ -0,0 +1,275 @@
+//=================================================================================================
+// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of the Flight Systems and Automatic Control group,
+//       TU Darmstadt, 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 HOLDER 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.
+//=================================================================================================
+
+#include <hector_quadrotor_controller/quadrotor_hardware_gazebo.h>
+
+#include <geometry_msgs/WrenchStamped.h>
+
+#ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.19
+#define _COUNT_TIME_HH_
+
+#ifndef _WIN32
+#include <sys/time.h>
+#endif
+
+#ifndef USE_COUNT_TIME
+#define USE_COUNT_TIME
+#endif
+
+#endif
+
+namespace hector_quadrotor_controller_gazebo
+{
+
+QuadrotorHardwareSim::QuadrotorHardwareSim()
+{
+  this->registerInterface(static_cast<QuadrotorInterface *>(this));
+
+  wrench_output_ = addInput<WrenchCommandHandle>("wrench");
+  motor_output_ = addInput<MotorCommandHandle>("motor");
+}
+
+QuadrotorHardwareSim::~QuadrotorHardwareSim()
+{
+}
+
+bool QuadrotorHardwareSim::initSim(
+    const std::string &robot_namespace,
+    ros::NodeHandle model_nh,
+    gazebo::physics::ModelPtr parent_model,
+    const urdf::Model *const urdf_model,
+    std::vector<transmission_interface::TransmissionInfo> transmissions)
+{
+  ros::NodeHandle param_nh(model_nh, "controller");
+
+  // store parent model pointer
+  model_ = parent_model;
+  link_ = model_->GetLink();
+  physics_ = model_->GetWorld()->GetPhysicsEngine();
+
+  model_nh.param<std::string>("world_frame", world_frame_, "world");
+  model_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
+
+  // subscribe state
+  std::string state_topic;
+  param_nh.getParam("state_topic", state_topic);
+  if (!state_topic.empty())
+  {
+    ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(state_topic, 1, boost::bind(&QuadrotorHardwareSim::stateCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
+    subscriber_state_ = model_nh.subscribe(ops);
+
+    gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_state_.getTopic() << "' as state input for control" << std::endl;
+  }
+  else
+  {
+    gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as state input for control" << std::endl;
+  }
+
+  // subscribe imu
+  std::string imu_topic;
+  param_nh.getParam("imu_topic", imu_topic);
+  if (!imu_topic.empty())
+  {
+    ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(imu_topic, 1, boost::bind(&QuadrotorHardwareSim::imuCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
+    subscriber_imu_ = model_nh.subscribe(ops);
+    gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_imu_.getTopic() << "' as imu input for control" << std::endl;
+  }
+  else
+  {
+    gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as imu input for control" << std::endl;
+  }
+
+  // subscribe motor_status
+  {
+    ros::SubscribeOptions ops = ros::SubscribeOptions::create<hector_uav_msgs::MotorStatus>("motor_status", 1, boost::bind(&QuadrotorHardwareSim::motorStatusCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
+    subscriber_motor_status_ = model_nh.subscribe(ops);
+  }
+
+  // publish wrench
+  {
+    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>("command/wrench", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
+    publisher_wrench_command_ = model_nh.advertise(ops);
+  }
+
+  // publish motor command
+  {
+    ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<hector_uav_msgs::MotorCommand>("command/motor", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
+    publisher_motor_command_ = model_nh.advertise(ops);
+  }
+
+  return true;
+}
+
+bool QuadrotorHardwareSim::getMassAndInertia(double &mass, double inertia[3])
+{
+  if (!link_)
+    return false;
+  mass = link_->GetInertial()->GetMass();
+  gazebo::math::Vector3 Inertia = link_->GetInertial()->GetPrincipalMoments();
+  inertia[0] = Inertia.x;
+  inertia[1] = Inertia.y;
+  inertia[2] = Inertia.z;
+  return true;
+}
+
+void QuadrotorHardwareSim::stateCallback(const nav_msgs::OdometryConstPtr &state)
+{
+  // calculate acceleration
+  if (!header_.stamp.isZero() && !state->header.stamp.isZero())
+  {
+    const double acceleration_time_constant = 0.1;
+    double dt((state->header.stamp - header_.stamp).toSec());
+    if (dt > 0.0)
+    {
+      acceleration_.x = ((state->twist.twist.linear.x - twist_.linear.x) + acceleration_time_constant * acceleration_.x) / (dt + acceleration_time_constant);
+      acceleration_.y = ((state->twist.twist.linear.y - twist_.linear.y) + acceleration_time_constant * acceleration_.y) / (dt + acceleration_time_constant);
+      acceleration_.z = ((state->twist.twist.linear.z - twist_.linear.z) + acceleration_time_constant * acceleration_.z) / (dt + acceleration_time_constant);
+    }
+  }
+
+  header_ = state->header;
+  pose_ = state->pose.pose;
+  twist_ = state->twist.twist;
+}
+
+void QuadrotorHardwareSim::imuCallback(const sensor_msgs::ImuConstPtr &imu)
+{
+  imu_ = *imu;
+}
+
+void QuadrotorHardwareSim::motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status)
+{
+  motor_status_ = *motor_status;
+}
+
+void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
+{
+  // call all available subscriber callbacks now
+  callback_queue_.callAvailable();
+
+  // read state from Gazebo
+  const double acceleration_time_constant = 0.1;
+  gz_pose_ = link_->GetWorldPose();
+  gz_acceleration_ = ((link_->GetWorldLinearVel() - gz_velocity_) + acceleration_time_constant * gz_acceleration_) / (period.toSec() + acceleration_time_constant);
+  gz_velocity_ = link_->GetWorldLinearVel();
+  gz_angular_velocity_ = link_->GetWorldAngularVel();
+
+  if (!subscriber_state_)
+  {
+    header_.frame_id = world_frame_;
+    header_.stamp = time;
+    pose_.position.x = gz_pose_.pos.x;
+    pose_.position.y = gz_pose_.pos.y;
+    pose_.position.z = gz_pose_.pos.z;
+    pose_.orientation.w = gz_pose_.rot.w;
+    pose_.orientation.x = gz_pose_.rot.x;
+    pose_.orientation.y = gz_pose_.rot.y;
+    pose_.orientation.z = gz_pose_.rot.z;
+    twist_.linear.x = gz_velocity_.x;
+    twist_.linear.y = gz_velocity_.y;
+    twist_.linear.z = gz_velocity_.z;
+    twist_.angular.x = gz_angular_velocity_.x;
+    twist_.angular.y = gz_angular_velocity_.y;
+    twist_.angular.z = gz_angular_velocity_.z;
+    acceleration_.x = gz_acceleration_.x;
+    acceleration_.y = gz_acceleration_.y;
+    acceleration_.z = gz_acceleration_.z;
+  }
+
+  if (!subscriber_imu_)
+  {
+    imu_.orientation.w = gz_pose_.rot.w;
+    imu_.orientation.x = gz_pose_.rot.x;
+    imu_.orientation.y = gz_pose_.rot.y;
+    imu_.orientation.z = gz_pose_.rot.z;
+
+    gazebo::math::Vector3 gz_angular_velocity_body = gz_pose_.rot.RotateVectorReverse(gz_angular_velocity_);
+    imu_.angular_velocity.x = gz_angular_velocity_body.x;
+    imu_.angular_velocity.y = gz_angular_velocity_body.y;
+    imu_.angular_velocity.z = gz_angular_velocity_body.z;
+
+    gazebo::math::Vector3 gz_linear_acceleration_body = gz_pose_.rot.RotateVectorReverse(gz_acceleration_ - physics_->GetGravity());
+    imu_.linear_acceleration.x = gz_linear_acceleration_body.x;
+    imu_.linear_acceleration.y = gz_linear_acceleration_body.y;
+    imu_.linear_acceleration.z = gz_linear_acceleration_body.z;
+  }
+}
+
+#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
+double QuadrotorHardwareSim_writeSim_time = 0.0;
+int num = 0;
+#endif
+
+void QuadrotorHardwareSim::writeSim(ros::Time time, ros::Duration period)
+{
+#ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19
+  struct timeval tv;
+  double start_time, cur_time, tmp_time;
+  gettimeofday(&tv, NULL);
+  start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
+#endif
+  bool result_written = false;
+
+  if (motor_output_->connected() && motor_output_->enabled())
+  {
+    publisher_motor_command_.publish(motor_output_->getCommand());
+    result_written = true;
+  }
+
+  if (wrench_output_->connected() && wrench_output_->enabled())
+  {
+    geometry_msgs::WrenchStamped wrench;
+    wrench.header.stamp = time;
+    wrench.header.frame_id = base_link_frame_;
+    wrench.wrench = wrench_output_->getCommand();
+    publisher_wrench_command_.publish(wrench);
+
+    if (!result_written)
+    {
+      // std::cout << "-----------QuadrotorHardwareSim------------" << std::endl;  //added by zhangshuai@2018.12.20
+      gazebo::math::Vector3 force(wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.force.z);
+      gazebo::math::Vector3 torque(wrench.wrench.torque.x, wrench.wrench.torque.y, wrench.wrench.torque.z);
+      link_->AddRelativeForce(force);
+      link_->AddRelativeTorque(torque - link_->GetInertial()->GetCoG().Cross(force));
+    }
+  }
+#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
+  // gettimeofday(&tv, NULL);
+  // QuadrotorHardwareSim_writeSim_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
+  // num++;
+  // if (num == 100000)
+  // {
+  //   std::cout << "QuadrotorHardwareSim_writeSim_time: " << QuadrotorHardwareSim_writeSim_time << "num:" << num << std::endl;
+  // }
+#endif
+}
+
+} // namespace hector_quadrotor_controller_gazebo
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller_gazebo::QuadrotorHardwareSim, gazebo_ros_control::RobotHWSim)

Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CHANGELOG.rst

@@ -0,0 +1,25 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_quadrotor_demo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.5 (2015-03-28)
+------------------
+
+0.3.4 (2015-02-22)
+------------------
+
+0.3.3 (2014-09-01)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-12-26)
+------------------
+* updated demo launch files
+* updated rviz configs
+* Contributors: Johannes Meyer
+
+0.3.0 (2013-09-11)
+------------------
+* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack

Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hector_quadrotor_demo)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package()
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS @{name} @{name}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY rviz_cfg DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+

Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/indoor_slam_gazebo.launch

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+
+<launch>
+   
+  <!-- Start Gazebo with wg world running in (max) realtime -->
+  <include file="$(find hector_gazebo_worlds)/launch/willow_garage.launch"/>
+   
+  <!-- Spawn simulated quadrotor uav -->
+  <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch" >
+    <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/> 
+  </include>
+   
+  <!-- Start SLAM system -->
+  <include file="$(find hector_mapping)/launch/mapping_default.launch">
+    <arg name="odom_frame" value="world"/>
+  </include>
+ 
+  <!-- Start GeoTIFF mapper -->
+  <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch">
+    <arg name="trajectory_publish_rate" value="4"/> 
+  </include>
+  
+  <!-- Start rviz visualization with preset config -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_quadrotor_demo)/rviz_cfg/indoor_slam.rviz"/>
+   
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<launch>
+   
+  <!-- Start Gazebo with wg world running in (max) realtime -->
+  <include file="$(find hector_gazebo_worlds)/launch/rolling_landscape_120m.launch"/>
+   
+  <!-- Spawn simulated quadrotor uav -->
+  <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch" >
+    <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/> 
+  </include>
+  
+  <!-- Start rviz visualization with preset config -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_quadrotor_demo)/rviz_cfg/outdoor_flight.rviz"/>
+   
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_demo/package.xml

@@ -0,0 +1,28 @@
+<package>
+  <name>hector_quadrotor_demo</name>
+  <version>0.3.5</version>
+  <description>hector_quadrotor_demo contains various launch files and needed dependencies for demonstration of the hector_quadrotor stack (indoor/outdoor flight in gazebo etc.)</description>
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_quadrotor_demo</url>
+  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
+
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>hector_quadrotor_gazebo</run_depend>
+  <run_depend>hector_gazebo_worlds</run_depend>
+  <run_depend>hector_mapping</run_depend>
+  <run_depend>hector_geotiff</run_depend>
+  <run_depend>hector_trajectory_server</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+
+</package>

Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/indoor_slam.rviz

@@ -0,0 +1,207 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.5
+    Tree Height: 460
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_cam_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_cam_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser0_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        sonar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.7
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 2
+        Min Value: 0
+        Value: false
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /front_cam/camera/image
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+      Visibility:
+        Grid: false
+        LaserScan: false
+        Map: false
+        Path: true
+        RobotModel: false
+        Value: true
+      Zoom Factor: 1
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 170; 0; 0
+      Enabled: true
+      Name: Path
+      Topic: /trajectory
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 7.23194
+      Focal Point:
+        X: 1.66903
+        Y: -0.217464
+        Z: -0.674569
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.625399
+      Target Frame: base_stabilized
+      Value: Orbit (rviz)
+      Yaw: 2.89041
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1026
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000012e0000035ffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000025b000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002a2000000fe0000001600fffffffb0000000a0049006d00610067006501000002d9000000c70000000000000000000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000410000035f000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065f0000003efc0100000002fb0000000800540069006d006501000000000000065f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000052b0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1631
+  X: 49
+  Y: 24

Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/outdoor_flight.rviz

@@ -0,0 +1,192 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /LaserScan1/Autocompute Value Bounds1
+        - /Camera1/Visibility1
+      Splitter Ratio: 0.5
+    Tree Height: 408
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_cam_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_cam_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser0_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        sonar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 2
+        Min Value: 0
+        Value: false
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 10
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /front_cam/camera/image
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+      Visibility:
+        Grid: false
+        LaserScan: true
+        RobotModel: false
+        Value: true
+      Zoom Factor: 1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 28.8231
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.630398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.16541
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 1026
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000012e0000035ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000227000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026e000001320000001600ffffff000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065f0000003efc0100000002fb0000000800540069006d006501000000000000065f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000052b0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1631
+  X: 49
+  Y: 24

Flight_control/src/hector_quadrotor/hector_quadrotor_description/CHANGELOG.rst

@@ -0,0 +1,22 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_quadrotor_description
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.5 (2015-03-28)
+------------------
+
+0.3.4 (2015-02-22)
+------------------
+
+0.3.3 (2014-09-01)
+------------------
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-12-26)
+------------------
+
+0.3.0 (2013-09-11)
+------------------
+* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack

Flight_control/src/hector_quadrotor/hector_quadrotor_description/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hector_quadrotor_description)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package()
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS @{name} @{name}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/launch/xacrodisplay_quadrotor_base.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+
+<launch>
+	<param name="robot_description" command="$(find xacro)/xacro $(find hector_quadrotor_description)/urdf/quadrotor.urdf.xacro" />
+	<param name="use_gui" value="True"/>
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="rviz" pkg="rviz" type="rviz" />
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.gazebo.xacro

@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/box.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.urdf.xacro

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+
+<robot
+  name="quadrotor"
+  xmlns:xacro="http://www.ros.org/wiki/xacro"
+>
+
+<!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/box_base.urdf.xacro" />
+
+<!-- Instantiate box_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />

Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box_base.urdf.xacro

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find hector_sensors_description)/urdf/sonar_sensor.urdf.xacro" />
+  <xacro:property name="pi" value="3.1415926535897931" />
+
+ <xacro:macro name="quadrotor_base_macro">
+    <link name="base_link">
+      <inertial>
+        <mass value="1.477" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.01152" ixy="0.0" ixz="0.0" iyy="0.01152" iyz="0.0" izz="0.0218" />
+      </inertial>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector_quadrotor_description/meshes/quadrotor/quadrotor_base.dae"/>
+        </geometry>
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl"/>
+        </geometry>
+      </collision>
+ </link>
+
+
+
+
+ <!-- Main box link -->
+<!-- <xacro:macro name="box_base_macro">
+<link name="base_link">
+<inertial>
+        <mass value="1.0" />
+        <inertia ixx="0.083" ixy="0.0" ixz="0.0" iyy="0.083" iyz="0.0" izz="0.083" />
+      </inertial>
+<visual>
+ <geometry>
+ <box size="1 1 1"/>
+ </geometry>
+ </visual>
+<collision>
+<geometry>
+<box size="1 1 1"/>
+ </geometry>
+</collision>
+</link>
+-->
+
+
+  <!-- Sonar height sensor -->
+    <xacro:sonar_sensor name="sonar" parent="base_link" ros_topic="sonar_height" update_rate="10" min_range="0.03" max_range="3.0" field_of_view="${40*pi/180}" ray_count="3">
+      <origin xyz="-0.16 0.0 -0.012" rpy="0 ${90*pi/180} 0"/>
+    </xacro:sonar_sensor>
+
+  </xacro:macro>
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/package.xml

@@ -0,0 +1,25 @@
+<package>
+  <name>hector_quadrotor_description</name>
+  <version>0.3.5</version>
+  <description>hector_quadrotor_description provides an URDF model of a quadrotor UAV.</description>
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_quadrotor_description</url>
+  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
+
+  <author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>hector_sensors_description</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+
+</package>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="box" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/box.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.urdf.xacro

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+
+<robot
+  name="box"
+  xmlns:xacro="http://www.ros.org/wiki/xacro"
+>
+
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/box_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:box_base_macro />
+
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box_base.urdf.xacro

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find hector_sensors_description)/urdf/sonar_sensor.urdf.xacro" />
+  <xacro:property name="pi" value="3.1415926535897931" />
+
+  <!-- Main quadrotor link -->
+  <xacro:macro name="box_base_macro">
+<link name="base_link">
+<inertial>
+        <mass value="2.0" />
+        <inertia ixx="0.01494" ixy="0.0" ixz="0.0" iyy="0.01494" iyz="0.0" izz="0.01494" />
+      </inertial>
+<visual>
+ <geometry>
+ <box size="0.3 0.3 0.3"/>
+ </geometry>
+ </visual>
+<collision>
+<geometry>
+<box size="0.3 0.3 0.3"/>
+ </geometry>
+</collision>
+</link>
+
+
+
+   
+
+  </xacro:macro>
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/my_robot.urdf

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<robot name="test_robot">
+<link name="link1" />
+<link name="link2" />
+<link name="link3" />
+<link name="link4" />
+
+<joint name="joint1" type="continuous">
+<parent link="link1"/>
+<child link="link2"/>
+<origin xyz="5 3 0" rpy="0 0 0" />
+<axis xyz="-0.9 0.15 0" />
+</joint>
+
+<joint name="joint2" type="continuous">
+<parent link="link1"/>
+<child link="link3"/>
+<origin xyz="-2 5 0" rpy="0 0 1.57" />
+<axis xyz="-0.707 0.707 0" />
+ </joint>
+
+<joint name="joint3" type="continuous">
+ <parent link="link3"/>
+<child link="link4"/>
+<origin xyz="5 0 0" rpy="0 0 -1.57" />
+<axis xyz="0.707 -0.707 0" />
+</joint>
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.urdf.xacro

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+
+<robot
+  name="quadrotor"
+  xmlns:xacro="http://www.ros.org/wiki/xacro"
+>
+
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/box.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.urdf.xacro

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+
+<robot
+  name="quadrotor"
+  xmlns:xacro="http://www.ros.org/wiki/xacro"
+>
+
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/box_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:box_base_macro />
+
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1_base.urdf.xacro

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find hector_sensors_description)/urdf/sonar_sensor.urdf.xacro" />
+  <xacro:property name="pi" value="3.1415926535897931" />
+
+  <!-- Main quadrotor link -->
+  <xacro:macro name="box_base_macro">
+<link name="base_link">
+<inertial>
+        <mass value="1.0" />
+        <inertia ixx="0.083" ixy="0.0" ixz="0.0" iyy="0.083" iyz="0.0" izz="0.083" />
+      </inertial>
+<visual>
+ <geometry>
+ <box size="1 1 1"/>
+ </geometry>
+ </visual>
+<collision>
+<geometry>
+<box size="1 1 1"/>
+ </geometry>
+</collision>
+</link>
+
+
+
+    <!-- Sonar height sensor -->
+    <xacro:sonar_sensor name="sonar" parent="base_link" ros_topic="sonar_height" update_rate="10" min_range="0.03" max_range="3.0" field_of_view="${40*pi/180}" ray_count="3">
+      <origin xyz="-0.16 0.0 -0.012" rpy="0 ${90*pi/180} 0"/>
+    </xacro:sonar_sensor>
+
+  </xacro:macro>
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.1.xacro

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find hector_sensors_description)/urdf/sonar_sensor.urdf.xacro" />
+  <xacro:property name="pi" value="3.1415926535897931" />
+
+  <!-- Main quadrotor link -->
+  <xacro:macro name="quadrotor_base_macro">
+    <link name="base_link">
+      <inertial>
+        <mass value="1.477" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.01152" ixy="0.0" ixz="0.0" iyy="0.01152" iyz="0.0" izz="0.0218" />
+      </inertial>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector_quadrotor_description/meshes/quadrotor/quadrotor_base.dae"/>
+        </geometry>
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl"/>
+        </geometry>
+      </collision>
+ </link>
+
+
+    <!-- Sonar height sensor -->
+    <xacro:sonar_sensor name="sonar" parent="base_link" ros_topic="sonar_height" update_rate="10" min_range="0.03" max_range="3.0" field_of_view="${40*pi/180}" ray_count="3">
+      <origin xyz="-0.16 0.0 -0.012" rpy="0 ${90*pi/180} 0"/>
+    </xacro:sonar_sensor>
+
+  </xacro:macro>
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.xacro

@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:property name="M_PI" value="3.1415926535897931" />
+  <xacro:include filename="$(find hector_sensors_description)/urdf/sonar_sensor.urdf.xacro" />
+  <xacro:property name="pi" value="3.1415926535897931" />
+
+  <!-- Main quadrotor link -->
+  <xacro:macro name="quadrotor_base_macro">
+    <link name="base_link">
+      <inertial>
+        <mass value="1.477" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.01152" ixy="0.0" ixz="0.0" iyy="0.01152" iyz="0.0" izz="0.0218" />
+      </inertial>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector_quadrotor_description/meshes/quadrotor/quadrotor_base.dae"/>
+        </geometry>
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+     <!-- <link name="camera_link">
+      <inertial>
+        <mass value="0.1" />        
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+      </inertial>
+      <collision name="collision">
+        <geometry>
+          <box size="0.1 0.1 0.05" />
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box size="0.1 0.1 0.05" />
+        </geometry>
+      </visual>   
+    </link>
+
+    <joint name="camera_joint" type="continuous">
+      <parent link="base_link"/>
+      <child link="camera_link"/>
+      <origin xyz="0.0 0.0 -0.1" rpy="${-M_PI*0.5} 0 0"/>
+      <axis xyz="0 0 1"/>
+    </joint> -->
+
+
+    <!-- Sonar height sensor -->
+    <xacro:sonar_sensor name="sonar" parent="base_link" ros_topic="sonar_height" update_rate="10" min_range="0.03" max_range="3.0" field_of_view="${40*pi/180}" ray_count="3">
+      <origin xyz="-0.16 0.0 -0.012" rpy="0 ${90*pi/180} 0"/>
+    </xacro:sonar_sensor>
+
+  </xacro:macro>
+</robot>

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.gazebo.xacro

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_downward_cam.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+   <!-- <xacro:quadrotor_plugins /> -->
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.urdf.xacro

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Downward facing camera -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/generic_camera.urdf.xacro" />
+    <xacro:generic_camera name="downward_cam" parent="base_link" ros_topic="camera/image" cam_info_topic="camera/camera_info" update_rate="10" res_x="1920" res_y="1080" image_format="R8G8B8" hfov="120" >
+      <origin xyz="0.4 0.0 -0.0" rpy="0 ${M_PI/2} 0"/>
+    </xacro:generic_camera>
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.gazebo.xacro

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_downward_nocam.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+   <!-- <xacro:quadrotor_plugins /> -->
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.urdf.xacro

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Downward facing camera -->
+    <!-- <xacro:include filename="$(find hector_sensors_description)/urdf/generic_camera.urdf.xacro" />
+    <xacro:generic_camera name="downward_cam" parent="base_link" ros_topic="camera/image" cam_info_topic="camera/camera_info" update_rate="10" res_x="1920" res_y="1080" image_format="R8G8B8" hfov="120" >
+      <origin xyz="0.4 0.0 -0.0" rpy="0 ${M_PI/2} 0"/>
+    </xacro:generic_camera> -->
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Hokuyo UTM-30LX mounted upside down below the quadrotor body -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/hokuyo_utm30lx.urdf.xacro" />
+    <xacro:hokuyo_utm30lx name="laser0" parent="base_link" ros_topic="scan" update_rate="40" ray_count="1081" min_angle="-135" max_angle="135">
+      <origin xyz="0.0 0.0 -0.097" rpy="${M_PI} 0 0"/>
+    </xacro:hokuyo_utm30lx>
+    
+    <!-- Forward facing camera -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/generic_camera.urdf.xacro" />
+    <xacro:generic_camera name="front_cam" parent="base_link" ros_topic="camera/image" cam_info_topic="camera/camera_info" update_rate="10" res_x="320" res_y="240" image_format="R8G8B8" hfov="90">
+      <origin xyz="0.05 0.0 -0.06" rpy="0 0 0"/>
+    </xacro:generic_camera>
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_with_asus.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.urdf.xacro

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Asus Xiton Live Pro -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/asus_camera.urdf.xacro" />
+    <xacro:asus_camera name="camera" parent="base_link">
+      <origin xyz="0.1 0.0 -0.03" rpy="0 ${M_PI/9} 0"/>
+    </xacro:asus_camera>
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Hokuyo UTM-30LX mounted upside down below the quadrotor body -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/hokuyo_utm30lx.urdf.xacro" />
+    <xacro:hokuyo_utm30lx name="laser0" parent="base_link" ros_topic="scan" update_rate="40" ray_count="1081" min_angle="-135" max_angle="135">
+      <origin xyz="0.0 0.0 -0.097" rpy="${M_PI} 0 0"/>
+    </xacro:hokuyo_utm30lx>
+    
+    <!-- Asus Xiton Live Pro -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/asus_camera.urdf.xacro" />
+    <xacro:asus_camera name="camera" parent="base_link">
+      <origin xyz="0.1 0.0 -0.03" rpy="0 ${M_PI/9} 0"/>
+    </xacro:asus_camera>
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_with_cam.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.urdf.xacro

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Forward facing camera -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/generic_camera.urdf.xacro" />
+    <xacro:generic_camera name="front_cam" parent="base_link" ros_topic="camera/image" cam_info_topic="camera/camera_info" update_rate="10" res_x="320" res_y="240" image_format="R8G8B8" hfov="120">
+      <origin xyz="0.4 0.0 -0.0" rpy="0 ${M_PI/2} 0"/>
+    </xacro:generic_camera>
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_with_cam1.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.urdf.xacro

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Forward facing camera -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/generic_camera.urdf.xacro" />
+    <xacro:generic_camera name="front_cam" parent="base_link" ros_topic="camera/image" cam_info_topic="camera/camera_info" update_rate="10" res_x="320" res_y="240" image_format="R8G8B8" hfov="90">
+      <origin xyz="0.4 0.0 -0.0" rpy="0 ${M_PI/2} 0"/>
+    </xacro:generic_camera>
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.gazebo.xacro

@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_with_kinect.urdf.xacro" />
+    <xacro:include filename="$(find hector_quadrotor_gazebo)/urdf/quadrotor_plugins.gazebo.xacro" />
+</robot>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.urdf.xacro

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+
+<robot name="quadrotor" xmlns:xacro="http://www.ros.org/wiki/xacro">
+   
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+    
+    <!-- Included URDF Files -->
+    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
+    
+    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
+    <xacro:quadrotor_base_macro />
+    
+    <!-- Kinect -->
+    <xacro:include filename="$(find hector_sensors_description)/urdf/kinect_camera.urdf.xacro" />
+    <xacro:kinect_camera name="camera" parent="base_link">
+      <origin xyz="0.05 0.0 -0.06" rpy="0 0 0"/>
+    </xacro:kinect_camera>
+    
+</robot>
+  

Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.gv

@@ -0,0 +1,14 @@
+digraph G {
+node [shape=box];
+"link1" [label="link1"];
+"link2" [label="link2"];
+"link3" [label="link3"];
+"link4" [label="link4"];
+node [shape=ellipse, color=blue, fontcolor=blue];
+"link1" -> "joint1" [label="xyz: 5 3 0 \nrpy: 0 -0 0"]
+"joint1" -> "link2"
+"link1" -> "joint2" [label="xyz: -2 5 0 \nrpy: 0 -0 1.57"]
+"joint2" -> "link3"
+"link3" -> "joint3" [label="xyz: 5 0 0 \nrpy: 0 0 -1.57"]
+"joint3" -> "link4"
+}

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/.gitignore

@@ -0,0 +1 @@
+urdf/quadrotor_plugins.gazebo.xacro

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CHANGELOG.rst

@@ -0,0 +1,55 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package hector_quadrotor_gazebo
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.5 (2015-03-28)
+------------------
+
+0.3.4 (2015-02-22)
+------------------
+* added missing run_depend hector_quadrotor_pose_estimation to package.xml
+* set pose_estimation/publish_world_nav_transform parameter to true explicitly
+* updated package for the latest version of hector_pose_estimation
+  * Geographic reference latitude and longitude set to 49.860246N 8.687077E (Lichtwiese).
+  * Reenabled auto_elevation, auto_reference and auto_heading parameters for hector_pose_estimation.
+  hector_pose_estimation will publish the world->nav transform depending on its reference pose.
+* added parameter file for hector_quadrotor_pose_estimation for a simulated quadrotor
+  The parameter file disables the auto_elevation, auto_reference, auto_heading modes of hector_pose_estimation and sets the corresponding values
+  manually with what is simulated in the gazebo sensor plugins. This simplifies the comparison of estimated poses with ground truth information.
+* explicitly set the pose_estimation/nav_frame parameter in spawn_quadrotor.launch
+* disabled detection of available plugins in cmake
+  The aerodynamics and propulsion plugins are built unconditinally now in hector_quadrotor_gazebo_plugins and the detection is obsolete.
+  Additionally we used platform-specific library prefixes and suffixes in find_libary() which caused errors on different platforms.
+* Contributors: Johannes Meyer
+
+0.3.3 (2014-09-01)
+------------------
+* cleaned up launch files and fixed argument forwarding to spawn_quadrotor.launch
+* removed all RTT related plugins
+* added separate update timer for MotorStatus output in propulsion plugin
+* added launch file argument to enable/disable pose estimation
+* moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo
+* Contributors: Johannes Meyer
+
+0.3.2 (2014-03-30)
+------------------
+
+0.3.1 (2013-12-26)
+------------------
+* also check if a target exists when searching available plugins
+* enables aerodynamics plugin in default configuration
+* limit controlPeriod to 100Hz
+* a few fixes for RTT integration in hector_quadrotor. Added urdf macro for rtt_gazebo_plugin macro.
+* deprecated quadrotor_simple_controller.gazebo.xacro
+* fixed node type for static_transform_publisher in spawn_quadrotor.launch
+* changed frame_id for gazebo fixed frame to /world and added a static_transform_publisher for world->nav
+* increased drift for the barometric pressure sensor
+* added some command input ports to quadrotor_controller.gazebo.xacro
+* Contributors: Johannes Meyer
+
+0.3.0 (2013-09-11)
+------------------
+* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack
+* increased drift for the barometric pressure sensor
+* added some command input ports to quadrotor_controller.gazebo.xacro
+* added launch file for two quadrotors

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hector_quadrotor_gazebo)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package()
+
+
+###########
+## Build ##
+###########
+
+add_subdirectory(urdf)
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS @{name} @{name}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/cxf_myspawn.launch

@@ -0,0 +1,204 @@
+<?xml version="1.0"?>
+<launch>
+<arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro" />
+<group ns="uav0">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav0" />
+<arg name="tf_prefix" value="uav0" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-53.64" />
+<arg name="y" value="-50.88" />
+</include>
+</group>
+
+<group ns="uav1">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav1" />
+<arg name="tf_prefix" value="uav1" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-29.32" />
+<arg name="y" value="-26.64" />
+</include>
+</group>
+
+<group ns="uav2">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav2" />
+<arg name="tf_prefix" value="uav2" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-53.48" />
+<arg name="y" value="-47.44" />
+</include>
+</group>
+
+<group ns="uav3">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav3" />
+<arg name="tf_prefix" value="uav3" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-35.12" />
+<arg name="y" value="-36.92" />
+</include>
+</group>
+
+<group ns="uav4">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav4" />
+<arg name="tf_prefix" value="uav4" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-56.8" />
+<arg name="y" value="-59" />
+</include>
+</group>
+
+<group ns="uav5">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav5" />
+<arg name="tf_prefix" value="uav5" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-54.4" />
+<arg name="y" value="-25.96" />
+</include>
+</group>
+
+<group ns="uav6">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav6" />
+<arg name="tf_prefix" value="uav6" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-51.24" />
+<arg name="y" value="-26.8" />
+</include>
+</group>
+
+<group ns="uav7">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav7" />
+<arg name="tf_prefix" value="uav7" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-50.84" />
+<arg name="y" value="-41.28" />
+</include>
+</group>
+
+<group ns="uav8">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav8" />
+<arg name="tf_prefix" value="uav8" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-45.36" />
+<arg name="y" value="-44.96" />
+</include>
+</group>
+
+<group ns="uav9">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav9" />
+<arg name="tf_prefix" value="uav9" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-51.4" />
+<arg name="y" value="-39.08" />
+</include>
+</group>
+
+<group ns="uav10">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav10" />
+<arg name="tf_prefix" value="uav10" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-22.04" />
+<arg name="y" value="-45.76" />
+</include>
+</group>
+
+<group ns="uav11">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav11" />
+<arg name="tf_prefix" value="uav11" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-25.36" />
+<arg name="y" value="-48.24" />
+</include>
+</group>
+
+<group ns="uav12">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav12" />
+<arg name="tf_prefix" value="uav12" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-40.48" />
+<arg name="y" value="-46.56" />
+</include>
+</group>
+
+<group ns="uav13">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav13" />
+<arg name="tf_prefix" value="uav13" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-57.6" />
+<arg name="y" value="-21.84" />
+</include>
+</group>
+
+<group ns="uav14">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav14" />
+<arg name="tf_prefix" value="uav14" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-24.44" />
+<arg name="y" value="-51.84" />
+</include>
+</group>
+
+<group ns="uav15">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav15" />
+<arg name="tf_prefix" value="uav15" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-48.76" />
+<arg name="y" value="-58.04" />
+</include>
+</group>
+
+<group ns="uav16">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav16" />
+<arg name="tf_prefix" value="uav16" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-42.72" />
+<arg name="y" value="-44" />
+</include>
+</group>
+
+<group ns="uav17">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav17" />
+<arg name="tf_prefix" value="uav17" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-50.6" />
+<arg name="y" value="-22.08" />
+</include>
+</group>
+
+<group ns="uav18">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav18" />
+<arg name="tf_prefix" value="uav18" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-31.4" />
+<arg name="y" value="-51.64" />
+</include>
+</group>
+
+<group ns="uav19">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav19" />
+<arg name="tf_prefix" value="uav19" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-39" />
+<arg name="y" value="-28.2" />
+</include>
+</group>
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/generate.cpp

@@ -0,0 +1,35 @@
+#include<iostream>
+#include<cstdlib>
+#include<ctime>
+#include<fstream>
+
+using namespace std;
+#define N 20
+double xmin=-60,xmax=-20,ymin=-60,ymax=-20;
+
+int main()
+{
+    srand(time(0));
+    ofstream fout("myspawn.launch");
+    fout<<"<?xml version=\"1.0\"?>"<<endl;
+    fout<<"<launch>"<<endl;
+    fout<<"<arg name=\"model\" default=\"$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro\" />"<<endl;
+    
+    for(int i=0;i<N;i++)
+    {
+        double x = rand()%1000/1000.0*(xmax-xmin)+xmin;
+        double y = rand()%1000/1000.0*(ymax-ymin)+ymin;
+        fout<<"<group ns=\"uav"<<i<<"\">"<<endl;
+        fout<<"<include file=\"$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch\">"<<endl;
+        fout<<"<arg name=\"name\" value=\"uav"<<i<<"\" />"<<endl;
+        fout<<"<arg name=\"tf_prefix\" value=\"uav"<<i<<"\" />"<<endl;
+        fout<<"<arg name=\"model\" value=\"$(arg model)\" />"<<endl;
+        fout<<"<arg name=\"x\" value=\""<<x<<"\" />"<<endl;
+        fout<<"<arg name=\"y\" value=\""<<y<<"\" />"<<endl;
+        fout<<"</include>"<<endl;
+        fout<<"</group>"<<endl;
+        fout<<endl;
+    }
+    
+    fout<<"</launch>"<<endl;
+}

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/myspawn.launch

@@ -0,0 +1,204 @@
+<?xml version="1.0"?>
+<launch>
+<arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro" />
+<group ns="uav0">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav0" />
+<arg name="tf_prefix" value="uav0" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-49.36" />
+<arg name="y" value="-50.68" />
+</include>
+</group>
+
+<group ns="uav1">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav1" />
+<arg name="tf_prefix" value="uav1" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-47.32" />
+<arg name="y" value="-44.8" />
+</include>
+</group>
+
+<group ns="uav2">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav2" />
+<arg name="tf_prefix" value="uav2" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-31" />
+<arg name="y" value="-46.16" />
+</include>
+</group>
+
+<group ns="uav3">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav3" />
+<arg name="tf_prefix" value="uav3" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-29.6" />
+<arg name="y" value="-35.88" />
+</include>
+</group>
+
+<group ns="uav4">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav4" />
+<arg name="tf_prefix" value="uav4" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-53.52" />
+<arg name="y" value="-59.04" />
+</include>
+</group>
+
+<group ns="uav5">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav5" />
+<arg name="tf_prefix" value="uav5" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-42.72" />
+<arg name="y" value="-59.92" />
+</include>
+</group>
+
+<group ns="uav6">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav6" />
+<arg name="tf_prefix" value="uav6" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-58.84" />
+<arg name="y" value="-40.76" />
+</include>
+</group>
+
+<group ns="uav7">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav7" />
+<arg name="tf_prefix" value="uav7" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-34.36" />
+<arg name="y" value="-50.12" />
+</include>
+</group>
+
+<group ns="uav8">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav8" />
+<arg name="tf_prefix" value="uav8" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-40.72" />
+<arg name="y" value="-50" />
+</include>
+</group>
+
+<group ns="uav9">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav9" />
+<arg name="tf_prefix" value="uav9" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-48.12" />
+<arg name="y" value="-54.24" />
+</include>
+</group>
+
+<group ns="uav10">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav10" />
+<arg name="tf_prefix" value="uav10" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-22.28" />
+<arg name="y" value="-39.08" />
+</include>
+</group>
+
+<group ns="uav11">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav11" />
+<arg name="tf_prefix" value="uav11" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-27.6" />
+<arg name="y" value="-30.56" />
+</include>
+</group>
+
+<group ns="uav12">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav12" />
+<arg name="tf_prefix" value="uav12" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-51.24" />
+<arg name="y" value="-57.92" />
+</include>
+</group>
+
+<group ns="uav13">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav13" />
+<arg name="tf_prefix" value="uav13" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-30.36" />
+<arg name="y" value="-50.96" />
+</include>
+</group>
+
+<group ns="uav14">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav14" />
+<arg name="tf_prefix" value="uav14" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-24.24" />
+<arg name="y" value="-38.56" />
+</include>
+</group>
+
+<group ns="uav15">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav15" />
+<arg name="tf_prefix" value="uav15" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-21.24" />
+<arg name="y" value="-53.6" />
+</include>
+</group>
+
+<group ns="uav16">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav16" />
+<arg name="tf_prefix" value="uav16" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-55.16" />
+<arg name="y" value="-34.48" />
+</include>
+</group>
+
+<group ns="uav17">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav17" />
+<arg name="tf_prefix" value="uav17" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-24.28" />
+<arg name="y" value="-26.16" />
+</include>
+</group>
+
+<group ns="uav18">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav18" />
+<arg name="tf_prefix" value="uav18" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-46.52" />
+<arg name="y" value="-59.8" />
+</include>
+</group>
+
+<group ns="uav19">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav19" />
+<arg name="tf_prefix" value="uav19" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-27.96" />
+<arg name="y" value="-25.96" />
+</include>
+</group>
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/nine_quadrotor_empty_world.launch

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="gui" value="$(arg gui)"/>
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <include file="$(find hector_quadrotor_gazebo)/launch/spawn_nine_quadrotors.launch" />
+<!--
+  <node name="hector_manager" pkg="micros_flocking" type="hector_manager" output="screen">
+  <param name='robotnum' value='3'/>
+  </node>
+  -->
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/quadrotor_empty_world.launch

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="gui" value="$(arg gui)"/>
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <include file="$(find hector_quadrotor_gazebo)/launch/spawn_three_quadrotors.launch" />
+<!--
+  <node name="hector_manager" pkg="micros_flocking" type="hector_manager" output="screen">
+  <param name='robotnum' value='3'/>
+  </node>
+  -->
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_box.launch

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="name" default="box"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/box.gazebo.xacro"/>
+  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+  <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
+
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.3"/>
+
+  <arg name="use_ground_truth_for_tf" default="true" />
+  <arg name="use_ground_truth_for_control" default="true" />
+  <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+  <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
+  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
+
+  <!-- send the robot XML to param server -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
+  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
+  <param name="world_frame" type="string" value="$(arg world_frame)"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
+  <!-- start robot state publisher -->
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node>
+
+  <!-- publish state and tf -->
+  <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+    <param name="odometry_topic" value="ground_truth/state" />
+    <param name="frame_id" value="/$(arg world_frame)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
+  </node>
+  <group if="$(arg use_pose_estimation)">
+    <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
+      <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
+      <param name="publish_world_nav_transform" value="true" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
+    </node>
+  </group>
+
+  <!-- spawn controller -->
+  <group if="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="" />
+    <param name="controller/imu_topic" value="" />
+  </group>
+  <group unless="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="state" />
+    <param name="controller/imu_topic" value="imu" />
+  </group>
+  <include file="$(arg controller_definition)" />
+
+  <arg name="motors" default="robbe_2827-34_epp1045" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_nine_quadrotors.launch

@@ -0,0 +1,97 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro" />
+
+   <group ns="uav1">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav1" />
+       <arg name="tf_prefix" value="uav1" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="0" />
+       <arg name="y" value="-5" />
+     </include>
+   </group>
+
+   <group ns="uav2">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav2" />
+       <arg name="tf_prefix" value="uav2" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="0" />
+       <arg name="y" value="-10" />
+     </include>
+   </group>
+
+   <group ns="uav0">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav0" />
+       <arg name="tf_prefix" value="uav0" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="0" />
+       <arg name="y" value="0" />
+     </include>
+   </group>
+   
+   <group ns="uav3">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav3" />
+       <arg name="tf_prefix" value="uav3" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="5" />
+       <arg name="y" value="0" />
+     </include>
+   </group>
+
+   <group ns="uav4">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav4" />
+       <arg name="tf_prefix" value="uav4" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="5" />
+       <arg name="y" value="-5" />
+     </include>
+   </group>
+
+   <group ns="uav5">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav5" />
+       <arg name="tf_prefix" value="uav5" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="5" />
+       <arg name="y" value="-10" />
+     </include>
+   </group>
+   
+   <group ns="uav6">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav6" />
+       <arg name="tf_prefix" value="uav6" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="10" />
+       <arg name="y" value="0" />
+     </include>
+   </group>
+
+   <group ns="uav7">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav7" />
+       <arg name="tf_prefix" value="uav7" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="10" />
+       <arg name="y" value="-5" />
+     </include>
+   </group>
+
+   <group ns="uav8">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav8" />
+       <arg name="tf_prefix" value="uav8" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="10" />
+       <arg name="y" value="-10" />
+     </include>
+   </group>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor (另一个复件).launch

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="name" default="quadrotor"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro"/>
+  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+  <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
+
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.3"/>
+
+  <arg name="use_ground_truth_for_tf" default="true" />
+  <arg name="use_ground_truth_for_control" default="true" />
+  <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+  <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
+  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
+
+  <!-- send the robot XML to param server -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
+  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
+  <param name="world_frame" type="string" value="$(arg world_frame)"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
+  <!-- start robot state publisher -->
+  <!-- <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node> -->
+
+  <!-- publish state and tf -->
+  <!-- <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+    <param name="odometry_topic" value="ground_truth/state" />
+    <param name="frame_id" value="/$(arg world_frame)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
+  </node>
+  <group if="$(arg use_pose_estimation)">
+    <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
+      <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
+      <param name="publish_world_nav_transform" value="true" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
+    </node>
+  </group> -->
+
+  <!-- spawn controller -->
+  <group if="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="" />
+    <param name="controller/imu_topic" value="" />
+  </group>
+  <group unless="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="state" />
+    <param name="controller/imu_topic" value="imu" />
+  </group>
+  <include file="$(arg controller_definition)" />
+
+  <arg name="motors" default="robbe_2827-34_epp1045" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(backup).launch

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="name" default="quadrotor"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro"/>
+  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+  <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
+
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.3"/>
+
+  <arg name="use_ground_truth_for_tf" default="true" />
+  <arg name="use_ground_truth_for_control" default="true" />
+  <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+  <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
+  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
+
+  <!-- send the robot XML to param server -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
+  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
+  <param name="world_frame" type="string" value="$(arg world_frame)"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
+  <!-- start robot state publisher -->
+  <!-- <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node> -->
+
+  <!-- publish state and tf -->
+  <!-- <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+    <param name="odometry_topic" value="ground_truth/state" />
+    <param name="frame_id" value="/$(arg world_frame)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
+  </node>
+  <group if="$(arg use_pose_estimation)">
+    <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
+      <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
+      <param name="publish_world_nav_transform" value="true" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
+    </node>
+  </group> -->
+
+  <!-- spawn controller -->
+  <group if="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="" />
+    <param name="controller/imu_topic" value="" />
+  </group>
+  <group unless="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="state" />
+    <param name="controller/imu_topic" value="imu" />
+  </group>
+  <include file="$(arg controller_definition)" />
+
+  <arg name="motors" default="robbe_2827-34_epp1045" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(zenglei).launch

@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="name" default="quadrotor"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro"/>
+  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+ <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
+
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.3"/>
+
+  <arg name="use_ground_truth_for_tf" default="true" />
+  <arg name="use_ground_truth_for_control" default="true" />
+  <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+  <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
+  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
+
+  <!-- send the robot XML to param server -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
+  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
+  <param name="world_frame" type="string" value="$(arg world_frame)"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
+  <!-- start robot state publisher -->
+  <!-- <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node> -->
+
+  <!-- publish state and tf -->
+  <!-- <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+    <param name="odometry_topic" value="ground_truth/state" />
+    <param name="frame_id" value="/$(arg world_frame)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
+  </node> -->
+  <group if="$(arg use_pose_estimation)">
+    <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
+      <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
+      <param name="publish_world_nav_transform" value="true" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
+    </node>
+  </group>
+
+  <!-- spawn controller -->
+  <group if="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="" />
+    <param name="controller/imu_topic" value="" />
+  </group>
+  <group unless="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="state" />
+    <param name="controller/imu_topic" value="imu" />
+  </group>
+  <!-- <include file="$(arg controller_definition)" /> -->
+  <rosparam file="$(find hector_quadrotor_controller)/params/controller.yaml" />
+
+  <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
+    controller/twist-->
+     <!--shutdown-timeout 3"/>  -->
+  <arg name="motors" default="robbe_2827-34_epp1045" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="name" default="quadrotor"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/box.gazebo.xacro"/>
+  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+  <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller.launch"/>
+
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.3"/>
+
+  <arg name="use_ground_truth_for_tf" default="true" />
+  <arg name="use_ground_truth_for_control" default="true" />
+  <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+  <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
+  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
+
+  <!-- send the robot XML to param server -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
+  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
+  <param name="world_frame" type="string" value="$(arg world_frame)"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
+  <!-- start robot state publisher -->
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node>
+
+  <!-- publish state and tf -->
+  <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+    <param name="odometry_topic" value="ground_truth/state" />
+    <param name="frame_id" value="/$(arg world_frame)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
+  </node>
+  <group if="$(arg use_pose_estimation)">
+    <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
+      <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
+      <param name="publish_world_nav_transform" value="true" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
+    </node>
+  </group>
+
+  <!-- spawn controller -->
+  <group if="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="" />
+    <param name="controller/imu_topic" value="" />
+  </group>
+  <group unless="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="state" />
+    <param name="controller/imu_topic" value="imu" />
+  </group>
+  <include file="$(arg controller_definition)" />
+
+  <arg name="motors" default="robbe_2827-34_epp1045" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" />
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_slam.launch

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="name" default="quadrotor"/>
+   <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/>
+   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+   <arg name="x" default="0.0"/>
+   <arg name="y" default="0.0"/>
+   <arg name="z" default="0.3"/>
+
+   <arg name="use_ground_truth_for_tf" default="true"/>
+   <arg name="use_ground_truth_for_control" default="true"/>
+   <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+   <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+     <arg name="name" default="$(arg name)"/>
+     <arg name="model" value="$(arg model)"/>
+     <arg name="tf_prefix" default="$(arg tf_prefix)"/>
+     <arg name="x" default="$(arg x)"/>
+     <arg name="y" default="$(arg y)"/>
+     <arg name="z" default="$(arg z)"/>   
+     <arg name="use_ground_truth_for_tf" value="$(arg use_ground_truth_for_tf)"/>
+     <arg name="use_ground_truth_for_control" value="$(arg use_ground_truth_for_control)"/>
+     <arg name="use_pose_estimation" value="$(arg use_pose_estimation)"/>
+   </include>
+   
+   <include file="$(find hector_slam_launch)/launch/tutorial.launch"/>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_split.launch

@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="name" default="quadrotor"/>
+  <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro"/>
+  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+  <arg name="controller_definition" default="$(find hector_quadrotor_controller)/launch/controller_split.launch"/>
+
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.3"/>
+
+  <arg name="use_ground_truth_for_tf" default="true" />
+  <arg name="use_ground_truth_for_control" default="true" />
+  <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+  <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+  
+  <arg name="world_frame" default="world"/> <!-- This should actually be "/world". See https://github.com/ros-simulation/gazebo_ros_pkgs/pull/324 -->
+  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
+
+  <!-- send the robot XML to param server -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(arg model)' base_link_frame:=$(arg base_link_frame) world_frame:=$(arg world_frame)" />
+  <param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
+  <param name="base_link_frame" type="string" value="$(arg base_link_frame)"/>
+  <param name="world_frame" type="string" value="$(arg world_frame)"/>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model"
+        args="-param robot_description
+           -urdf
+           -x $(arg x)
+           -y $(arg y)
+           -z $(arg z)
+           -model $(arg name)"
+        respawn="false" output="screen"/>
+
+  <!-- start robot state publisher -->
+  <!-- <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node> -->
+
+  <!-- publish state and tf -->
+  <!-- <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
+    <param name="odometry_topic" value="ground_truth/state" />
+    <param name="frame_id" value="/$(arg world_frame)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
+    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
+  </node> -->
+  <!-- <group if="$(arg use_pose_estimation)">
+    <node name="pose_estimation" pkg="hector_quadrotor_pose_estimation" type="hector_quadrotor_pose_estimation" output="screen">
+      <rosparam file="$(find hector_quadrotor_pose_estimation)/params/simulation.yaml" />
+      <param name="nav_frame" value="$(arg tf_prefix)/nav" />
+      <param name="publish_world_nav_transform" value="true" />
+      <param name="tf_prefix" value="$(arg tf_prefix)" unless="$(arg use_ground_truth_for_tf)" />
+      <param name="tf_prefix" value="$(arg tf_prefix)/pose_estimation" if="$(arg use_ground_truth_for_tf)" />
+    </node>
+  </group> -->
+
+  <!-- spawn controller -->
+  <!-- <group if="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="" />
+    <param name="controller/imu_topic" value="" />
+  </group>
+  <group unless="$(arg use_ground_truth_for_control)">
+    <param name="controller/state_topic" value="state" />
+    <param name="controller/imu_topic" value="imu" />
+  </group> -->
+  <!-- <include file="$(arg controller_definition)" /> -->
+  <!-- <rosparam file="$(find hector_quadrotor_controller)/params/controller.yaml" /> -->
+
+  <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
+    controller/twist -->
+     <!--shutdown-timeout 3"/>-->
+  <!-- <arg name="motors" default="robbe_2827-34_epp1045" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/quadrotor_aerodynamics.yaml" />
+  <rosparam command="load" file="$(find hector_quadrotor_model)/param/$(arg motors).yaml" /> -->
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="name" default="quadrotor"/>
+   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+   <arg name="x" default="0.0"/>
+   <arg name="y" default="0.0"/>
+   <arg name="z" default="0.3"/>
+
+   <arg name="use_ground_truth_for_tf" default="true"/>
+   <arg name="use_ground_truth_for_control" default="true"/>
+   <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+   <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+     <arg name="name" default="$(arg name)"/>
+     <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_with_asus.gazebo.xacro"/>
+     <arg name="tf_prefix" default="$(arg tf_prefix)"/>
+     <arg name="x" default="$(arg x)"/>
+     <arg name="y" default="$(arg y)"/>
+     <arg name="z" default="$(arg z)"/>   
+     <arg name="use_ground_truth_for_tf" value="$(arg use_ground_truth_for_tf)"/>
+     <arg name="use_ground_truth_for_control" value="$(arg use_ground_truth_for_control)"/>
+     <arg name="use_pose_estimation" value="$(arg use_pose_estimation)"/>
+   </include>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus_with_laser.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="name" default="quadrotor"/>
+   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+   <arg name="x" default="0.0"/>
+   <arg name="y" default="0.0"/>
+   <arg name="z" default="0.3"/>
+
+   <arg name="use_ground_truth_for_tf" default="true"/>
+   <arg name="use_ground_truth_for_control" default="true"/>
+   <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+   <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+     <arg name="name" default="$(arg name)"/>
+     <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro"/>
+     <arg name="tf_prefix" default="$(arg tf_prefix)"/>
+     <arg name="x" default="$(arg x)"/>
+     <arg name="y" default="$(arg y)"/>
+     <arg name="z" default="$(arg z)"/>   
+     <arg name="use_ground_truth_for_tf" value="$(arg use_ground_truth_for_tf)"/>
+     <arg name="use_ground_truth_for_control" value="$(arg use_ground_truth_for_control)"/>
+     <arg name="use_pose_estimation" value="$(arg use_pose_estimation)"/>
+   </include>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_cam.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="name" default="quadrotor"/>
+   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+   <arg name="x" default="0.0"/>
+   <arg name="y" default="0.0"/>
+   <arg name="z" default="0.3"/>
+
+   <arg name="use_ground_truth_for_tf" default="true"/>
+   <arg name="use_ground_truth_for_control" default="true"/>
+   <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+   <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+     <arg name="name" default="$(arg name)"/>
+     <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_with_cam.gazebo.xacro"/>
+     <arg name="tf_prefix" default="$(arg tf_prefix)"/>
+     <arg name="x" default="$(arg x)"/>
+     <arg name="y" default="$(arg y)"/>
+     <arg name="z" default="$(arg z)"/>   
+     <arg name="use_ground_truth_for_tf" value="$(arg use_ground_truth_for_tf)"/>
+     <arg name="use_ground_truth_for_control" value="$(arg use_ground_truth_for_control)"/>
+     <arg name="use_pose_estimation" value="$(arg use_pose_estimation)"/>
+   </include>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="name" default="quadrotor"/>
+   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+   <arg name="x" default="0.0"/>
+   <arg name="y" default="0.0"/>
+   <arg name="z" default="0.3"/>
+
+   <arg name="use_ground_truth_for_tf" default="true"/>
+   <arg name="use_ground_truth_for_control" default="true"/>
+   <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+   <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+     <arg name="name" default="$(arg name)"/>
+     <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_downward_cam.gazebo.xacro"/>
+     <arg name="tf_prefix" default="$(arg tf_prefix)"/>
+     <arg name="x" default="$(arg x)"/>
+     <arg name="y" default="$(arg y)"/>
+     <arg name="z" default="$(arg z)"/>   
+     <arg name="use_ground_truth_for_tf" value="$(arg use_ground_truth_for_tf)"/>
+     <arg name="use_ground_truth_for_control" value="$(arg use_ground_truth_for_control)"/>
+     <arg name="use_pose_estimation" value="$(arg use_pose_estimation)"/>
+   </include>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_kinect.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="name" default="quadrotor"/>
+   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+   <arg name="x" default="0.0"/>
+   <arg name="y" default="0.0"/>
+   <arg name="z" default="0.3"/>
+
+   <arg name="use_ground_truth_for_tf" default="true"/>
+   <arg name="use_ground_truth_for_control" default="true"/>
+   <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+   <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+     <arg name="name" default="$(arg name)"/>
+     <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_with_kinect.gazebo.xacro"/>
+     <arg name="tf_prefix" default="$(arg tf_prefix)"/>
+     <arg name="x" default="$(arg x)"/>
+     <arg name="y" default="$(arg y)"/>
+     <arg name="z" default="$(arg z)"/>   
+     <arg name="use_ground_truth_for_tf" value="$(arg use_ground_truth_for_tf)"/>
+     <arg name="use_ground_truth_for_control" value="$(arg use_ground_truth_for_control)"/>
+     <arg name="use_pose_estimation" value="$(arg use_pose_estimation)"/>
+   </include>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_laser.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="name" default="quadrotor"/>
+   <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
+   <arg name="x" default="0.0"/>
+   <arg name="y" default="0.0"/>
+   <arg name="z" default="0.3"/>
+
+   <arg name="use_ground_truth_for_tf" default="true"/>
+   <arg name="use_ground_truth_for_control" default="true"/>
+   <arg name="use_pose_estimation" if="$(arg use_ground_truth_for_control)" default="false"/>
+   <arg name="use_pose_estimation" unless="$(arg use_ground_truth_for_control)" default="true"/>
+
+   <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+     <arg name="name" default="$(arg name)"/>
+     <arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/>
+     <arg name="tf_prefix" default="$(arg tf_prefix)"/>
+     <arg name="x" default="$(arg x)"/>
+     <arg name="y" default="$(arg y)"/>
+     <arg name="z" default="$(arg z)"/>   
+     <arg name="use_ground_truth_for_tf" value="$(arg use_ground_truth_for_tf)"/>
+     <arg name="use_ground_truth_for_control" value="$(arg use_ground_truth_for_control)"/>
+     <arg name="use_pose_estimation" value="$(arg use_pose_estimation)"/>
+   </include>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_ten_quadrotors.launch

@@ -0,0 +1,107 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro" />
+
+   <group ns="uav1">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav1" />
+       <arg name="tf_prefix" value="uav1" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="y" value="-25.84" />
+       <arg name="x" value="-41.6" />
+     </include>
+   </group>
+
+   <group ns="uav2">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav2" />
+       <arg name="tf_prefix" value="uav2" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="14.24" />
+       <arg name="y" value="-16.96" />
+     </include>
+   </group>
+
+   <group ns="uav3">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav3" />
+       <arg name="tf_prefix" value="uav3" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="12.88" />
+       <arg name="y" value="7.12" />
+     </include>
+   </group>
+
+   <group ns="uav4">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav4" />
+       <arg name="tf_prefix" value="uav4" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="9.68" />
+       <arg name="y" value="25.52" />
+     </include>
+   </group>
+
+   <group ns="uav5">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav5" />
+       <arg name="tf_prefix" value="uav5" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="y" value="-11.6" />
+       <arg name="x" value="-19.12" />
+     </include>
+   </group>
+
+   <group ns="uav6">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav6" />
+       <arg name="tf_prefix" value="uav6" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="y" value="-35.6" />
+       <arg name="x" value="-30.48" />
+     </include>
+   </group>
+
+   <group ns="uav7">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav7" />
+       <arg name="tf_prefix" value="uav7" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="y" value="31.76" />
+       <arg name="x" value="-12.64" />
+     </include>
+   </group>
+
+   <group ns="uav8">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav8" />
+       <arg name="tf_prefix" value="uav8" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="15.84" />
+       <arg name="y" value="4.32" />
+     </include>
+   </group>
+
+   <group ns="uav9">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav9" />
+       <arg name="tf_prefix" value="uav9" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="17.2" />
+       <arg name="y" value="-28.72" />
+     </include>
+   </group>
+
+   <group ns="uav0">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav10" />
+       <arg name="tf_prefix" value="uav10" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="-44.8" />
+       <arg name="y" value="-30.72" />
+     </include>
+   </group>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_three_quadrotors.launch

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro" />
+
+   <group ns="uav1">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav1" />
+       <arg name="tf_prefix" value="uav1" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="0" />
+       <arg name="y" value="-5" />
+     </include>
+   </group>
+
+   <group ns="uav2">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav2" />
+       <arg name="tf_prefix" value="uav2" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="0" />
+       <arg name="y" value="-10" />
+     </include>
+   </group>
+
+   <group ns="uav0">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav0" />
+       <arg name="tf_prefix" value="uav0" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="x" value="0" />
+       <arg name="y" value="0" />
+       <arg name="z" value="0" />
+     </include>
+   </group>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_two_quadrotors.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+
+<launch>
+   <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro" />
+
+   <group ns="uav1">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav1" />
+       <arg name="tf_prefix" value="uav1" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="y" value="-1.0" />
+     </include>
+   </group>
+
+   <group ns="uav2">
+     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+       <arg name="name" value="uav2" />
+       <arg name="tf_prefix" value="uav2" />
+       <arg name="model" value="$(arg model)" />
+       <arg name="y" value="1.0" />
+     </include>
+   </group>
+
+</launch>
+

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/water_myspawn.launch

@@ -0,0 +1,204 @@
+<?xml version="1.0"?>
+<launch>
+<arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor.gazebo.xacro" />
+<group ns="uav0">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav0" />
+<arg name="tf_prefix" value="uav0" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-40.76" />
+<arg name="y" value="-20.84" />
+</include>
+</group>
+
+<group ns="uav1">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav1" />
+<arg name="tf_prefix" value="uav1" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-49.96" />
+<arg name="y" value="-58.44" />
+</include>
+</group>
+
+<group ns="uav2">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav2" />
+<arg name="tf_prefix" value="uav2" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-48.72" />
+<arg name="y" value="-28.44" />
+</include>
+</group>
+
+<group ns="uav3">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav3" />
+<arg name="tf_prefix" value="uav3" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-21.08" />
+<arg name="y" value="-49.72" />
+</include>
+</group>
+
+<group ns="uav4">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav4" />
+<arg name="tf_prefix" value="uav4" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-57.36" />
+<arg name="y" value="-27.2" />
+</include>
+</group>
+
+<group ns="uav5">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav5" />
+<arg name="tf_prefix" value="uav5" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-44.24" />
+<arg name="y" value="-25.68" />
+</include>
+</group>
+
+<group ns="uav6">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav6" />
+<arg name="tf_prefix" value="uav6" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-20.44" />
+<arg name="y" value="-39.4" />
+</include>
+</group>
+
+<group ns="uav7">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav7" />
+<arg name="tf_prefix" value="uav7" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-29.16" />
+<arg name="y" value="-56.72" />
+</include>
+</group>
+
+<group ns="uav8">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav8" />
+<arg name="tf_prefix" value="uav8" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-57.88" />
+<arg name="y" value="-55.36" />
+</include>
+</group>
+
+<group ns="uav9">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav9" />
+<arg name="tf_prefix" value="uav9" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-21.64" />
+<arg name="y" value="-28.56" />
+</include>
+</group>
+
+<group ns="uav10">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav10" />
+<arg name="tf_prefix" value="uav10" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-35.44" />
+<arg name="y" value="-33.4" />
+</include>
+</group>
+
+<group ns="uav11">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav11" />
+<arg name="tf_prefix" value="uav11" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-54.6" />
+<arg name="y" value="-33.88" />
+</include>
+</group>
+
+<group ns="uav12">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav12" />
+<arg name="tf_prefix" value="uav12" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-50.84" />
+<arg name="y" value="-30.92" />
+</include>
+</group>
+
+<group ns="uav13">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav13" />
+<arg name="tf_prefix" value="uav13" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-48.64" />
+<arg name="y" value="-46.36" />
+</include>
+</group>
+
+<group ns="uav14">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav14" />
+<arg name="tf_prefix" value="uav14" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-37.12" />
+<arg name="y" value="-26.6" />
+</include>
+</group>
+
+<group ns="uav15">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav15" />
+<arg name="tf_prefix" value="uav15" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-29.04" />
+<arg name="y" value="-43.8" />
+</include>
+</group>
+
+<group ns="uav16">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav16" />
+<arg name="tf_prefix" value="uav16" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-27.44" />
+<arg name="y" value="-59" />
+</include>
+</group>
+
+<group ns="uav17">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav17" />
+<arg name="tf_prefix" value="uav17" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-42.24" />
+<arg name="y" value="-42.04" />
+</include>
+</group>
+
+<group ns="uav18">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav18" />
+<arg name="tf_prefix" value="uav18" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-53.36" />
+<arg name="y" value="-43.32" />
+</include>
+</group>
+
+<group ns="uav19">
+<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
+<arg name="name" value="uav19" />
+<arg name="tf_prefix" value="uav19" />
+<arg name="model" value="$(arg model)" />
+<arg name="x" value="-31.76" />
+<arg name="y" value="-50.68" />
+</include>
+</group>
+
+</launch>

Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/package.xml

@@ -0,0 +1,44 @@
+<package>
+  <name>hector_quadrotor_gazebo</name>
+  <version>0.3.5</version>
+  <description>hector_quadrotor_gazebo provides a quadrotor model for the gazebo simulator.
+     It can be commanded using geometry_msgs/Twist messages.</description>
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_quadrotor_gazebo</url>
+  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
+
+  <author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>gazebo_plugins</build_depend>
+  <build_depend>hector_gazebo_plugins</build_depend>
+  <build_depend>hector_sensors_gazebo</build_depend>
+  <build_depend>hector_quadrotor_gazebo_plugins</build_depend>
+  <build_depend>hector_quadrotor_controller_gazebo</build_depend>
+  <build_depend>message_to_tf</build_depend>
+  <build_depend>robot_state_publisher</build_depend>
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>gazebo_plugins</run_depend>
+  <run_depend>hector_gazebo_plugins</run_depend>
+  <run_depend>hector_sensors_gazebo</run_depend>
+  <run_depend>hector_quadrotor_description</run_depend>
+  <run_depend>hector_quadrotor_model</run_depend>
+  <run_depend>hector_quadrotor_gazebo_plugins</run_depend>
+  <run_depend>hector_quadrotor_controller_gazebo</run_depend>
+  <run_depend>hector_quadrotor_pose_estimation</run_depend>
+  <run_depend>hector_quadrotor_teleop</run_depend>
+  <run_depend>hector_uav_msgs</run_depend>
+  <run_depend>message_to_tf</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+
+</package>