├── .gitignore ├── CMakeLists.txt ├── hector_quadrotor.rosinstall ├── hector_quadrotor ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── hector_quadrotor_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_quadrotor_controller │ │ ├── handles.h │ │ ├── pid.h │ │ └── quadrotor_interface.h ├── launch │ └── controller.launch ├── package.xml ├── params │ └── controller.yaml ├── plugin.xml └── src │ ├── motor_controller.cpp │ ├── pid.cpp │ ├── pose_controller.cpp │ ├── quadrotor_interface.cpp │ └── twist_controller.cpp ├── hector_quadrotor_controller_gazebo ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_quadrotor_controller │ │ └── quadrotor_hardware_gazebo.h ├── package.xml ├── quadrotor_controller_gazebo.xml └── src │ └── quadrotor_hardware_gazebo.cpp ├── hector_quadrotor_demo ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── AoA_localization.launch │ ├── att_estimation_gazebo.launch │ ├── indoor_slam_gazebo.launch │ ├── irobots.launch │ ├── laser_localization.launch │ ├── mono_slam_downward.launch │ ├── mono_slam_forward.launch │ ├── multi_quadrotors.launch │ ├── optical_flow_sim.launch │ ├── orb_slam2.launch │ ├── outdoor_flight_gazebo.launch │ ├── outdoor_flight_gazebo_down_cam_laser.launch │ ├── outdoor_flight_gazebo_down_thermaleye_cam_laser.launch │ ├── outdoor_flight_stereo.launch │ ├── outdoor_flight_stereo_laser.launch │ ├── outdoor_flight_with_downward_cam_spinning_lidar.launch │ ├── outdoor_flight_with_kinect.launch │ ├── outdoor_flight_with_kinect_laser.launch │ ├── pose_ekf.launch │ ├── quadrotor_irobots.launch │ ├── quadrotor_laser.launch │ ├── rss_lcoalization.launch │ ├── stereo_vo.launch │ ├── velodyne.launch │ ├── vision_box.launch │ ├── wifi_estimator_sim.launch │ └── wireless_receiver_gazebo.launch ├── package.xml └── rviz_cfg │ ├── att_estimation.rviz │ ├── indoor_slam.rviz │ ├── outdoor_flight.rviz │ ├── outdoor_flight_down_cam.rviz │ ├── outdoor_flight_down_cam_laser.rviz │ ├── outdoor_flight_down_thermaleye_cam.rviz │ ├── outdoor_flight_stereo_cam.rviz │ ├── outdoor_flight_with_kinect.rviz │ ├── outdoor_flight_with_kinect_laser.rviz │ ├── pose_ekf.rviz │ ├── quadrotor_with_laser.rviz │ ├── quadrotor_with_velodyne.rviz │ ├── quadrotor_with_velodyne2.rviz │ ├── rss_localization.rviz │ ├── spinning_lidar_cam.rviz │ ├── velodyne.rviz │ ├── velodyne2.rviz │ ├── vision_box.rviz │ ├── wifi_estimator.rviz │ └── wireless_receiver.rviz ├── hector_quadrotor_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ └── xacrodisplay_quadrotor_base.launch ├── meshes │ ├── irobot │ │ └── create_body.dae │ └── quadrotor │ │ ├── blender │ │ └── quadrotor_base.blend │ │ ├── quadrotor_base.dae │ │ └── quadrotor_base.stl ├── package.xml └── urdf │ ├── irobot.gazebo.xacro │ ├── irobot.urdf.xacro │ ├── irobot_base.urdf.xacro │ ├── quadrotor.gazebo.xacro │ ├── quadrotor.urdf.xacro │ ├── quadrotor_base.urdf.xacro │ ├── quadrotor_downward_cam.gazebo.xacro │ ├── quadrotor_downward_cam.urdf.xacro │ ├── quadrotor_downward_cam_laser.gazebo.xacro │ ├── quadrotor_downward_cam_laser.urdf.xacro │ ├── quadrotor_downward_cam_spinning_lidar.gazebo.xacro │ ├── quadrotor_downward_cam_spinning_lidar.urdf.xacro │ ├── quadrotor_downward_stereo_cam.gazebo.xacro │ ├── quadrotor_downward_stereo_cam.urdf.xacro │ ├── quadrotor_downward_stereo_cam_laser.gazebo.xacro │ ├── quadrotor_downward_stereo_cam_laser.urdf.xacro │ ├── quadrotor_downward_thermaleye_cam_laser.gazebo.xacro │ ├── quadrotor_downward_thermaleye_cam_laser.urdf.xacro │ ├── quadrotor_hokuyo_utm30lx.gazebo.xacro │ ├── quadrotor_hokuyo_utm30lx.urdf.xacro │ ├── quadrotor_with_asus.gazebo.xacro │ ├── quadrotor_with_asus.urdf.xacro │ ├── quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro │ ├── quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro │ ├── quadrotor_with_cam.gazebo.xacro │ ├── quadrotor_with_cam.urdf.xacro │ ├── quadrotor_with_cam_wireless_receiver.gazebo.xacro │ ├── quadrotor_with_cam_wireless_receiver.urdf.xacro │ ├── quadrotor_with_kinect.gazebo.xacro │ ├── quadrotor_with_kinect.urdf.xacro │ ├── quadrotor_with_kinect_laser.gazebo.xacro │ ├── quadrotor_with_kinect_laser.urdf.xacro │ ├── quadrotor_with_laser.gazebo.xacro │ ├── quadrotor_with_laser.urdf.xacro │ ├── quadrotor_with_velodyne.gazebo.xacro │ ├── quadrotor_with_velodyne.urdf.xacro │ ├── quadrotor_with_vision_box.gazebo.xacro │ ├── quadrotor_with_vision_box.urdf.xacro │ ├── quadrotor_with_wireless_receiver.gazebo.xacro │ └── quadrotor_with_wireless_receiver.urdf.xacro ├── hector_quadrotor_gazebo ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── quadrotor_empty_world.launch │ ├── spawn_irobot.launch │ ├── spawn_irobot2.launch │ ├── spawn_multiple_irobots.launch │ ├── spawn_quadrotor.launch │ ├── spawn_quadrotor_slam.launch │ ├── spawn_quadrotor_with_asus.launch │ ├── spawn_quadrotor_with_asus_with_laser.launch │ ├── spawn_quadrotor_with_cam.launch │ ├── spawn_quadrotor_with_downward_cam.launch │ ├── spawn_quadrotor_with_downward_cam_laser.launch │ ├── spawn_quadrotor_with_downward_cam_spinning_lidar.launch │ ├── spawn_quadrotor_with_downward_stereo_cam.launch │ ├── spawn_quadrotor_with_downward_thermaleye_cam_laser.launch │ ├── spawn_quadrotor_with_kinect.launch │ ├── spawn_quadrotor_with_kinect_laser.launch │ ├── spawn_quadrotor_with_laser.launch │ ├── spawn_quadrotor_with_velodyne.launch │ ├── spawn_quadrotor_with_vision_box.launch │ ├── spawn_quadrotor_with_wireless_receiver.launch │ └── spawn_two_quadrotors.launch ├── package.xml └── urdf │ ├── CMakeLists.txt │ ├── quadrotor_aerodynamics.gazebo.xacro │ ├── quadrotor_controller.gazebo.xacro │ ├── quadrotor_plugins.gazebo.xacro.in │ ├── quadrotor_propulsion.gazebo.xacro │ └── quadrotor_sensors.gazebo.xacro ├── hector_quadrotor_gazebo_plugins ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_quadrotor_gazebo_plugins │ │ ├── gazebo_quadrotor_aerodynamics.h │ │ ├── gazebo_quadrotor_propulsion.h │ │ ├── gazebo_quadrotor_simple_controller.h │ │ └── gazebo_ros_baro.h ├── package.xml └── src │ ├── gazebo_quadrotor_aerodynamics.cpp │ ├── gazebo_quadrotor_propulsion.cpp │ ├── gazebo_quadrotor_simple_controller.cpp │ └── gazebo_ros_baro.cpp ├── hector_quadrotor_model ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_quadrotor_model │ │ ├── helpers.h │ │ ├── quadrotor_aerodynamics.h │ │ └── quadrotor_propulsion.h ├── matlab │ ├── CMakeLists.txt │ ├── compile_all.m │ ├── dummy.c │ ├── motorspeed.m │ ├── parameter_QK_propulsion.m │ ├── quadrotorDrag.m │ └── quadrotorPropulsion.m ├── package.xml ├── param │ ├── quadrotor_aerodynamics.yaml │ ├── robbe_2827-34_epp1045.yaml │ └── robbe_2827-34_epp1245.yaml └── src │ ├── matlab_helpers.h │ ├── quadrotor_aerodynamics.cpp │ └── quadrotor_propulsion.cpp ├── hector_quadrotor_pose_estimation ├── CHANGELOG.rst ├── CMakeLists.txt ├── hector_quadrotor_pose_estimation_nodelets.xml ├── include │ └── hector_quadrotor_pose_estimation │ │ └── pose_estimation_node.h ├── package.xml ├── params │ └── simulation.yaml └── src │ ├── main.cpp │ ├── pose_estimation_node.cpp │ └── pose_estimation_nodelet.cpp ├── hector_quadrotor_teleop ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── draw_circle.launch │ ├── draw_rect.launch │ ├── hovering.launch │ ├── logitech_gamepad.launch │ └── xbox_controller.launch ├── package.xml └── src │ ├── draw_circle.cpp │ ├── draw_rect.cpp │ ├── quadrotor_obstacle_avoidance.cpp │ ├── quadrotor_teleop.cpp │ ├── quadrotor_teleop_random.cpp │ ├── quadrotor_vfh.cpp │ └── spinning_lidar_control.cpp ├── hector_uav_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── hector_uav_msgs │ │ ├── Altimeter │ │ └── pressure_height.h │ │ ├── ControlSource.h │ │ └── RC │ │ └── functions.h ├── msg │ ├── Altimeter.msg │ ├── AttitudeCommand.msg │ ├── Compass.msg │ ├── ControllerState.msg │ ├── HeadingCommand.msg │ ├── HeightCommand.msg │ ├── MotorCommand.msg │ ├── MotorPWM.msg │ ├── MotorStatus.msg │ ├── PositionXYCommand.msg │ ├── RC.msg │ ├── RawImu.msg │ ├── RawMagnetic.msg │ ├── RawRC.msg │ ├── RuddersCommand.msg │ ├── ServoCommand.msg │ ├── Supply.msg │ ├── ThrustCommand.msg │ ├── VelocityXYCommand.msg │ ├── VelocityZCommand.msg │ └── YawrateCommand.msg └── package.xml ├── irobot_teleop ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ ├── logitech_gamepad.launch │ └── xbox_controller.launch ├── package.xml └── src │ └── irobot_random_walk.cpp └── tutorials.rosinstall /.gitignore: -------------------------------------------------------------------------------- 1 | # QtCreator settings 2 | *.user 3 | *.user.* 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/indigo/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /hector_quadrotor.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: indigo-devel} 2 | - git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin} 3 | - git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: indigo-devel} 4 | - git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: indigo-devel} 5 | -------------------------------------------------------------------------------- /hector_quadrotor/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | * moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo 14 | * Contributors: Johannes Meyer 15 | 16 | 0.3.2 (2014-03-30) 17 | ------------------ 18 | * added packages hector_quadrotor_controller and hector_quadrotor_controller_gazebo to meta-package 19 | * Contributors: Johannes Meyer 20 | 21 | 0.3.1 (2013-12-26) 22 | ------------------ 23 | 24 | 0.3.0 (2013-09-11) 25 | ------------------ 26 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 27 | -------------------------------------------------------------------------------- /hector_quadrotor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /hector_quadrotor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor 3 | 0.3.5 4 | hector_quadrotor contains packages related to modeling, control and simulation of quadrotor UAV systems 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | Stefan Kohlbrecher 14 | 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | 22 | hector_quadrotor_controller 23 | hector_quadrotor_description 24 | hector_quadrotor_model 25 | hector_quadrotor_teleop 26 | hector_uav_msgs 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /hector_quadrotor_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | * updated angular/z controller parameters 8 | * Remove redundant callback queue in twist_controller, use root_nh queue as per ros_control API 9 | * Add controller timeout to allow faster shutdown of spawner 10 | * Contributors: Johannes Meyer, Paul Bovbel 11 | 12 | 0.3.4 (2015-02-22) 13 | ------------------ 14 | * improved automatic landing detection and shutdown on rollovers 15 | * slightly updated velocity controller limits and gains 16 | * Contributors: Johannes Meyer 17 | 18 | 0.3.3 (2014-09-01) 19 | ------------------ 20 | * fixed some compiler warnings and missing return values 21 | * increased integral gain for attitude stabilization (fix #12) 22 | * make a copy of the root NodeHandle in all controllers 23 | For some reason deconstructing the TwistController resulted in a pure virtual function call without this patch. 24 | * Contributors: Johannes Meyer 25 | 26 | 0.3.2 (2014-03-30) 27 | ------------------ 28 | * Fix boost 1.53 issues 29 | changed boost::shared_dynamic_cast to boost::dynamic_pointer_cast and 30 | boost::shared_static_cast to boost::static_pointer_cast 31 | * use a separate callback queue thread for the TwistController 32 | * added optional twist limit in pose controller 33 | * Contributors: Christopher Hrabia, Johannes Meyer 34 | 35 | 0.3.1 (2013-12-26) 36 | ------------------ 37 | * New controller implementation using ros_control 38 | * Added pose controller 39 | * Added motor controller that controls motor voltages from wrench commands 40 | * Contributors: Johannes Meyer 41 | 42 | 0.3.0 (2013-09-11) 43 | ------------------ 44 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 45 | * Propulsion and aerodynamics models are in hector_quadrotor_model package now. 46 | * Gazebo plugins are in hector_quadrotor_gazebo_plugins package now. 47 | -------------------------------------------------------------------------------- /hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef HECTOR_QUADROTOR_CONTROLLER_PID_H 2 | #define HECTOR_QUADROTOR_CONTROLLER_PID_H 3 | 4 | #include 5 | 6 | namespace hector_quadrotor_controller { 7 | 8 | class PID 9 | { 10 | public: 11 | struct parameters { 12 | parameters(); 13 | bool enabled; 14 | double time_constant; 15 | double k_p; 16 | double k_i; 17 | double k_d; 18 | double limit_i; 19 | double limit_output; 20 | } parameters_; 21 | 22 | struct state { 23 | state(); 24 | double p, i, d; 25 | double input, dinput; 26 | double dx; 27 | } state_; 28 | 29 | public: 30 | PID(); 31 | PID(const parameters& parameters); 32 | ~PID(); 33 | 34 | void init(const ros::NodeHandle ¶m_nh); 35 | void reset(); 36 | 37 | double update(double input, double x, double dx, const ros::Duration& dt); 38 | double update(double error, double dx, const ros::Duration& dt); 39 | 40 | double getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt); 41 | }; 42 | 43 | } // namespace hector_quadrotor_controller 44 | 45 | #endif // HECTOR_QUADROTOR_CONTROLLER_PID_H 46 | -------------------------------------------------------------------------------- /hector_quadrotor_controller/launch/controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_controller 3 | 0.3.5 4 | hector_quadrotor_controller provides libraries and a node for quadrotor control using ros_control. 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor_controller 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | 14 | 15 | catkin 16 | 17 | 18 | roscpp 19 | geometry_msgs 20 | sensor_msgs 21 | nav_msgs 22 | hector_uav_msgs 23 | std_srvs 24 | hardware_interface 25 | controller_interface 26 | 27 | 28 | roscpp 29 | geometry_msgs 30 | sensor_msgs 31 | nav_msgs 32 | hector_uav_msgs 33 | std_srvs 34 | hardware_interface 35 | controller_interface 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /hector_quadrotor_controller/params/controller.yaml: -------------------------------------------------------------------------------- 1 | controller: 2 | pose: 3 | type: hector_quadrotor_controller/PoseController 4 | xy: 5 | k_p: 2.0 6 | k_i: 0.0 7 | k_d: 0.0 8 | limit_output: 5.0 9 | z: 10 | k_p: 2.0 11 | k_i: 0.0 12 | k_d: 0.0 13 | limit_output: 5.0 14 | yaw: 15 | k_p: 2.0 16 | k_i: 0.0 17 | k_d: 0.0 18 | limit_output: 1.0 19 | twist: 20 | type: hector_quadrotor_controller/TwistController 21 | linear/xy: 22 | k_p: 5.0 23 | k_i: 1.0 24 | k_d: 0.0 25 | limit_output: 10.0 26 | time_constant: 0.05 27 | linear/z: 28 | k_p: 5.0 29 | k_i: 1.0 30 | k_d: 0.0 31 | limit_output: 10.0 32 | time_constant: 0.05 33 | angular/xy: 34 | k_p: 10.0 35 | k_i: 5.0 36 | k_d: 5.0 37 | time_constant: 0.01 38 | angular/z: 39 | k_p: 5.0 40 | k_i: 2.5 41 | k_d: 0.0 42 | limit_output: 3.0 43 | time_constant: 0.1 44 | limits: 45 | load_factor: 1.5 46 | force/z: 30.0 47 | torque/xy: 10.0 48 | torque/z: 1.0 49 | motor: 50 | type: hector_quadrotor_controller/MotorController 51 | -------------------------------------------------------------------------------- /hector_quadrotor_controller/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | The PoseController controls the quadrotor's pose and outputs velocity commands. 6 | 7 | 8 | 9 | 10 | 11 | 12 | The TwistController controls the quadrotor's twist (linear and angular velocity) by applying torque and thrust similar to a real quadrotor. 13 | 14 | 15 | 16 | 17 | 18 | 19 | The MotorController calculates the output voltages of the four motors from the commanded thrust and torque using a simple linearized model. 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /hector_quadrotor_controller_gazebo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_controller_gazebo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-12-26) 18 | ------------------ 19 | * New controller implementation using ros_control 20 | * Contributors: Johannes Meyer 21 | -------------------------------------------------------------------------------- /hector_quadrotor_controller_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_quadrotor_controller_gazebo 4 | 0.3.5 5 | The hector_quadrotor_controller_gazebo package implements the ros_control RobotHWSim interface for the quadrotor controller in package hector_quadrotor_controller. 6 | Johannes Meyer 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/hector_quadrotor_controller_gazebo 11 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 12 | 13 | Johannes Meyer 14 | 15 | catkin 16 | gazebo_ros_control 17 | hector_quadrotor_controller 18 | gazebo_ros_control 19 | hector_quadrotor_controller 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This class represents the quadrotor hardware in Gazebo for the gazebo_ros_control plugin. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_demo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-12-26) 18 | ------------------ 19 | * updated demo launch files 20 | * updated rviz configs 21 | * Contributors: Johannes Meyer 22 | 23 | 0.3.0 (2013-09-11) 24 | ------------------ 25 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 26 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_demo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | ## Uncomment this if the package has a setup.py. This macro ensures 13 | ## modules and global scripts declared therein get installed 14 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 15 | # catkin_python_setup() 16 | 17 | 18 | ################################### 19 | ## catkin specific configuration ## 20 | ################################### 21 | ## The catkin_package macro generates cmake config files for your package 22 | ## Declare things to be passed to dependent projects 23 | ## INCLUDE_DIRS: uncomment this if you package contains header files 24 | ## LIBRARIES: libraries you create in this project that dependent projects also need 25 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 26 | ## DEPENDS: system dependencies of this project that dependent projects also need 27 | catkin_package() 28 | 29 | 30 | ############# 31 | ## Install ## 32 | ############# 33 | 34 | # all install targets should use catkin DESTINATION variables 35 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 36 | 37 | ## Mark executable scripts (Python etc.) for installation 38 | ## in contrast to setup.py, you can choose the destination 39 | # install(PROGRAMS 40 | # scripts/my_python_script 41 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 42 | # ) 43 | 44 | ## Mark executables and/or libraries for installation 45 | # install(TARGETS @{name} @{name}_node 46 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 47 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 49 | # ) 50 | 51 | ## Mark cpp header files for installation 52 | # install(DIRECTORY include/${PROJECT_NAME}/ 53 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 54 | # FILES_MATCHING PATTERN "*.h" 55 | # PATTERN ".svn" EXCLUDE 56 | # ) 57 | 58 | ## Mark other files for installation (e.g. launch and bag files, etc.) 59 | # install(FILES 60 | # # myfile1 61 | # # myfile2 62 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 63 | # ) 64 | 65 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 66 | install(DIRECTORY rviz_cfg DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 67 | 68 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/AoA_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/att_estimation_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/indoor_slam_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/irobots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/laser_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/mono_slam_downward.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/mono_slam_forward.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/multi_quadrotors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/optical_flow_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/orb_slam2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_gazebo_down_cam_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_gazebo_down_thermaleye_cam_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_stereo_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_with_downward_cam_spinning_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_with_kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/outdoor_flight_with_kinect_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/pose_ekf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/quadrotor_irobots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/quadrotor_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/rss_lcoalization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/stereo_vo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/vision_box.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/wifi_estimator_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/launch/wireless_receiver_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /hector_quadrotor_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_demo 3 | 0.3.5 4 | hector_quadrotor_demo contains various launch files and needed dependencies for demonstration of the hector_quadrotor stack (indoor/outdoor flight in gazebo etc.) 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor_demo 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Stefan Kohlbrecher 13 | 14 | 15 | catkin 16 | 17 | 18 | 19 | 20 | hector_quadrotor_gazebo 21 | hector_gazebo_worlds 22 | hector_mapping 23 | hector_geotiff 24 | hector_trajectory_server 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-12-26) 18 | ------------------ 19 | 20 | 0.3.0 (2013-09-11) 21 | ------------------ 22 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 23 | -------------------------------------------------------------------------------- /hector_quadrotor_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | ## Uncomment this if the package has a setup.py. This macro ensures 13 | ## modules and global scripts declared therein get installed 14 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 15 | # catkin_python_setup() 16 | 17 | 18 | ################################### 19 | ## catkin specific configuration ## 20 | ################################### 21 | ## The catkin_package macro generates cmake config files for your package 22 | ## Declare things to be passed to dependent projects 23 | ## INCLUDE_DIRS: uncomment this if you package contains header files 24 | ## LIBRARIES: libraries you create in this project that dependent projects also need 25 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 26 | ## DEPENDS: system dependencies of this project that dependent projects also need 27 | catkin_package() 28 | 29 | 30 | ############# 31 | ## Install ## 32 | ############# 33 | 34 | # all install targets should use catkin DESTINATION variables 35 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 36 | 37 | ## Mark executable scripts (Python etc.) for installation 38 | ## in contrast to setup.py, you can choose the destination 39 | # install(PROGRAMS 40 | # scripts/my_python_script 41 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 42 | # ) 43 | 44 | ## Mark executables and/or libraries for installation 45 | # install(TARGETS @{name} @{name}_node 46 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 47 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 49 | # ) 50 | 51 | ## Mark cpp header files for installation 52 | # install(DIRECTORY include/${PROJECT_NAME}/ 53 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 54 | # FILES_MATCHING PATTERN "*.h" 55 | # PATTERN ".svn" EXCLUDE 56 | # ) 57 | 58 | ## Mark other files for installation (e.g. launch and bag files, etc.) 59 | # install(FILES 60 | # # myfile1 61 | # # myfile2 62 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 63 | # ) 64 | 65 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 66 | install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 67 | install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 68 | 69 | -------------------------------------------------------------------------------- /hector_quadrotor_description/launch/xacrodisplay_quadrotor_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /hector_quadrotor_description/meshes/quadrotor/blender/quadrotor_base.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/hector_quadrotor/f29ce7512b2909a6c78237e61c31598f5270e016/hector_quadrotor_description/meshes/quadrotor/blender/quadrotor_base.blend -------------------------------------------------------------------------------- /hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/hector_quadrotor/f29ce7512b2909a6c78237e61c31598f5270e016/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl -------------------------------------------------------------------------------- /hector_quadrotor_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_description 3 | 0.3.5 4 | hector_quadrotor_description provides an URDF model of a quadrotor UAV. 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor_description 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | Stefan Kohlbrecher 14 | 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | hector_sensors_description 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/irobot.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/irobot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_cam.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_cam.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_cam_laser.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_cam_laser.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_cam_spinning_lidar.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_cam_spinning_lidar.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_stereo_cam.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_stereo_cam.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_stereo_cam_laser.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_stereo_cam_laser.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_thermaleye_cam_laser.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_downward_thermaleye_cam_laser.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_asus.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_asus.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_cam.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_cam.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_cam_wireless_receiver.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_cam_wireless_receiver.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_kinect.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_kinect.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_kinect_laser.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_kinect_laser.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_laser.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_laser.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_velodyne.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_velodyne.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_vision_box.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_vision_box.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_wireless_receiver.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /hector_quadrotor_description/urdf/quadrotor_with_wireless_receiver.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 18 | 19 | 20 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/.gitignore: -------------------------------------------------------------------------------- 1 | urdf/quadrotor_plugins.gazebo.xacro 2 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_gazebo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | * added missing run_depend hector_quadrotor_pose_estimation to package.xml 11 | * set pose_estimation/publish_world_nav_transform parameter to true explicitly 12 | * updated package for the latest version of hector_pose_estimation 13 | * Geographic reference latitude and longitude set to 49.860246N 8.687077E (Lichtwiese). 14 | * Reenabled auto_elevation, auto_reference and auto_heading parameters for hector_pose_estimation. 15 | hector_pose_estimation will publish the world->nav transform depending on its reference pose. 16 | * added parameter file for hector_quadrotor_pose_estimation for a simulated quadrotor 17 | The parameter file disables the auto_elevation, auto_reference, auto_heading modes of hector_pose_estimation and sets the corresponding values 18 | manually with what is simulated in the gazebo sensor plugins. This simplifies the comparison of estimated poses with ground truth information. 19 | * explicitly set the pose_estimation/nav_frame parameter in spawn_quadrotor.launch 20 | * disabled detection of available plugins in cmake 21 | The aerodynamics and propulsion plugins are built unconditinally now in hector_quadrotor_gazebo_plugins and the detection is obsolete. 22 | Additionally we used platform-specific library prefixes and suffixes in find_libary() which caused errors on different platforms. 23 | * Contributors: Johannes Meyer 24 | 25 | 0.3.3 (2014-09-01) 26 | ------------------ 27 | * cleaned up launch files and fixed argument forwarding to spawn_quadrotor.launch 28 | * removed all RTT related plugins 29 | * added separate update timer for MotorStatus output in propulsion plugin 30 | * added launch file argument to enable/disable pose estimation 31 | * moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo 32 | * Contributors: Johannes Meyer 33 | 34 | 0.3.2 (2014-03-30) 35 | ------------------ 36 | 37 | 0.3.1 (2013-12-26) 38 | ------------------ 39 | * also check if a target exists when searching available plugins 40 | * enables aerodynamics plugin in default configuration 41 | * limit controlPeriod to 100Hz 42 | * a few fixes for RTT integration in hector_quadrotor. Added urdf macro for rtt_gazebo_plugin macro. 43 | * deprecated quadrotor_simple_controller.gazebo.xacro 44 | * fixed node type for static_transform_publisher in spawn_quadrotor.launch 45 | * changed frame_id for gazebo fixed frame to /world and added a static_transform_publisher for world->nav 46 | * increased drift for the barometric pressure sensor 47 | * added some command input ports to quadrotor_controller.gazebo.xacro 48 | * Contributors: Johannes Meyer 49 | 50 | 0.3.0 (2013-09-11) 51 | ------------------ 52 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 53 | * increased drift for the barometric pressure sensor 54 | * added some command input ports to quadrotor_controller.gazebo.xacro 55 | * added launch file for two quadrotors 56 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | ## Uncomment this if the package has a setup.py. This macro ensures 13 | ## modules and global scripts declared therein get installed 14 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 15 | # catkin_python_setup() 16 | 17 | 18 | ################################### 19 | ## catkin specific configuration ## 20 | ################################### 21 | ## The catkin_package macro generates cmake config files for your package 22 | ## Declare things to be passed to dependent projects 23 | ## INCLUDE_DIRS: uncomment this if you package contains header files 24 | ## LIBRARIES: libraries you create in this project that dependent projects also need 25 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 26 | ## DEPENDS: system dependencies of this project that dependent projects also need 27 | catkin_package() 28 | 29 | 30 | ########### 31 | ## Build ## 32 | ########### 33 | 34 | add_subdirectory(urdf) 35 | 36 | 37 | ############# 38 | ## Install ## 39 | ############# 40 | 41 | # all install targets should use catkin DESTINATION variables 42 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 43 | 44 | ## Mark executable scripts (Python etc.) for installation 45 | ## in contrast to setup.py, you can choose the destination 46 | # install(PROGRAMS 47 | # scripts/my_python_script 48 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 49 | # ) 50 | 51 | ## Mark executables and/or libraries for installation 52 | # install(TARGETS @{name} @{name}_node 53 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 56 | # ) 57 | 58 | ## Mark cpp header files for installation 59 | # install(DIRECTORY include/${PROJECT_NAME}/ 60 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 61 | # FILES_MATCHING PATTERN "*.h" 62 | # PATTERN ".svn" EXCLUDE 63 | # ) 64 | 65 | ## Mark other files for installation (e.g. launch and bag files, etc.) 66 | # install(FILES 67 | # # myfile1 68 | # # myfile2 69 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 70 | # ) 71 | 72 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 73 | install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 74 | 75 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/quadrotor_empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_irobot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_irobot2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 32 | 33 | 34 | 37 | 38 | 39 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_multiple_irobots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus_with_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam_spinning_lidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_stereo_cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_thermaleye_cam_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_kinect_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_vision_box.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_quadrotor_with_wireless_receiver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/launch/spawn_two_quadrotors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_gazebo 3 | 0.3.5 4 | hector_quadrotor_gazebo provides a quadrotor model for the gazebo simulator. 5 | It can be commanded using geometry_msgs/Twist messages. 6 | Johannes Meyer 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/hector_quadrotor_gazebo 11 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 12 | 13 | Johannes Meyer 14 | Stefan Kohlbrecher 15 | 16 | 17 | catkin 18 | 19 | 20 | gazebo_plugins 21 | hector_gazebo_plugins 22 | hector_sensors_gazebo 23 | hector_quadrotor_gazebo_plugins 24 | hector_quadrotor_controller_gazebo 25 | message_to_tf 26 | robot_state_publisher 27 | 28 | 29 | gazebo_plugins 30 | hector_gazebo_plugins 31 | hector_sensors_gazebo 32 | hector_quadrotor_description 33 | hector_quadrotor_model 34 | hector_quadrotor_gazebo_plugins 35 | hector_quadrotor_controller_gazebo 36 | hector_quadrotor_pose_estimation 37 | hector_quadrotor_teleop 38 | hector_uav_msgs 39 | message_to_tf 40 | robot_state_publisher 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/urdf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This CMakeLists.txt configures the plugins used for the quadrotor model. 2 | set(MODEL_PLUGINS) 3 | 4 | # configuration 5 | option(USE_EXTERNAL_CONTROLLER "Do not run the hector_quadrotor_controller_gazebo plugin." OFF) 6 | option(USE_PROPULSION_PLUGIN "Use a model of the quadrotor propulsion system" ON) 7 | option(USE_AERODYNAMICS_PLUGIN "Use a model of the quadrotor aerodynamics" ON) 8 | 9 | # sensor plugins 10 | list(APPEND MODEL_PLUGINS quadrotor_sensors) 11 | 12 | # controller plugin 13 | if(NOT USE_EXTERNAL_CONTROLLER) 14 | list(APPEND MODEL_PLUGINS quadrotor_controller) 15 | endif() 16 | 17 | # propulsion plugin 18 | #if(USE_PROPULSION_PLUGIN) 19 | # find_library(hector_gazebo_quadrotor_propulsion_LIBRARY hector_gazebo_quadrotor_propulsion) 20 | 21 | # if(NOT TARGET hector_gazebo_quadrotor_propulsion AND NOT hector_gazebo_quadrotor_propulsion_LIBRARY) 22 | # message(SEND_ERROR "Cannot use the propulsion model as the required Gazebo plugin 'hector_gazebo_quadrotor_propulsion' has not been found.") 23 | # set(USE_PROPULSION_PLUGIN OFF) 24 | # endif() 25 | #endif() 26 | if(USE_PROPULSION_PLUGIN) 27 | list(APPEND MODEL_PLUGINS quadrotor_propulsion) 28 | endif() 29 | 30 | # aerodynamics plugin 31 | #if(USE_AERODYNAMICS_PLUGIN) 32 | # find_library(hector_gazebo_quadrotor_aerodynamics_LIBRARY hector_gazebo_quadrotor_aerodynamics) 33 | 34 | # if(NOT TARGET hector_gazebo_quadrotor_aerodynamics AND NOT hector_gazebo_quadrotor_aerodynamics_LIBRARY) 35 | # message(SEND_ERROR "Cannot use the aerodynamics model as the required Gazebo plugin 'hector_gazebo_quadrotor_aerodynamics' has not been found.") 36 | # set(USE_AERODYNAMICS_PLUGIN OFF) 37 | # endif() 38 | #endif() 39 | if(USE_AERODYNAMICS_PLUGIN) 40 | list(APPEND MODEL_PLUGINS quadrotor_aerodynamics) 41 | endif() 42 | 43 | # write urdf 44 | set(MODEL_PLUGINS_URDF) 45 | foreach(PLUGIN ${MODEL_PLUGINS}) 46 | set(MODEL_PLUGINS_URDF "${MODEL_PLUGINS_URDF} \n") 47 | endforeach() 48 | configure_file(quadrotor_plugins.gazebo.xacro.in ${PROJECT_SOURCE_DIR}/urdf/quadrotor_plugins.gazebo.xacro @ONLY) 49 | set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES ${PROJECT_SOURCE_DIR}/urdf/quadrotor_plugins.gazebo.xacro) 50 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/urdf/quadrotor_aerodynamics.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | true 10 | 0.0 11 | base_link 12 | $(arg base_link_frame) 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/urdf/quadrotor_controller.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.01 7 | hector_quadrotor_controller_gazebo/QuadrotorHardwareSim 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro.in: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | @MODEL_PLUGINS_URDF@ 15 | 16 | 17 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo/urdf/quadrotor_propulsion.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | true 10 | 0.0 11 | base_link 12 | $(arg base_link_frame) 13 | 100.0 14 | 15 | 0.01 16 | 100.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo_plugins/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_gazebo_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | * added dynamic_reconfigure server to gazebo_ros_baro plugin 11 | See https://github.com/tu-darmstadt-ros-pkg/hector_gazebo/commit/e1698e1c7bfa5fce6a724ab0a922a88bd49c9733 for 12 | the equivalent commit in hector_gazebo_plugins. 13 | * publish propulsion and aerodynamic wrench as WrenchStamped 14 | This is primarily for debugging purposes. 15 | The default topic for the propulsion plugin has been changed to propulsion/wrench. 16 | * disabled detection of available plugins in cmake 17 | The aerodynamics and propulsion plugins are built unconditinally now in hector_quadrotor_gazebo_plugins and the detection is obsolete. 18 | Additionally we used platform-specific library prefixes and suffixes in find_libary() which caused errors on different platforms. 19 | * Contributors: Johannes Meyer 20 | 21 | 0.3.3 (2014-09-01) 22 | ------------------ 23 | * fixed some compiler warnings and missing return values 24 | * added separate update timer for MotorStatus output in propulsion plugin 25 | * Contributors: Johannes Meyer 26 | 27 | 0.3.2 (2014-03-30) 28 | ------------------ 29 | 30 | 0.3.1 (2013-12-26) 31 | ------------------ 32 | * disabled separate queue thread for the aerodynamics plugin 33 | * fixed configuration namespace and plugin cleanup 34 | * aerodynamics plugin should apply forces and torques in world frame 35 | * accept hector_uav_msgs/MotorCommand messages directly for the propulsion model/plugin 36 | * deleted deprecated export section from package.xml 37 | * abort with a fatal error if ROS is not yet initialized + minor code cleanup 38 | * fixed commanded linear z velocity upper bound for auto shutdown in simple controller plugin 39 | * improved auto shutdown to prevent shutdowns while airborne 40 | * added motor engage/shutdown, either automatically (default) or using ROS services /engage and /shutdown 41 | (std_srvs/Empty) 42 | * using ROS parameters to configure state topics 43 | * use controller_manager in gazebo_ros_control instead of running standalone pose_controller 44 | * Contributors: Johannes Meyer 45 | 46 | 0.3.0 (2013-09-11) 47 | ------------------ 48 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 49 | * added wrench publisher to the quadrotor_simple_controller plugin 50 | * created new package hector_quadrotor_model and moved all gazebo plugins to hector_quadrotor_gazebo_plugins 51 | -------------------------------------------------------------------------------- /hector_quadrotor_gazebo_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_gazebo_plugins 3 | 0.3.5 4 | hector_quadrotor_gazebo_plugins provides gazebo plugins for using quadrotors in gazebo. 5 | The hector_gazebo_ros_baro sensor plugin simulates an altimeter based on barometric pressure. 6 | hector_quadrotor_simple_controller is a simple controller allowing to command the quadrotor's velocity 7 | using a geometry_msgs/Twist message for teleoperation just by means of applying forces and torques to the model. 8 | Johannes Meyer 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/hector_quadrotor_gazebo_plugins 13 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 14 | 15 | Johannes Meyer 16 | 17 | 18 | catkin 19 | 20 | 21 | roscpp 22 | gazebo 23 | hector_gazebo_plugins 24 | hector_quadrotor_model 25 | geometry_msgs 26 | hector_uav_msgs 27 | std_srvs 28 | dynamic_reconfigure 29 | 30 | 31 | roscpp 32 | gazebo_ros 33 | hector_gazebo_plugins 34 | hector_quadrotor_model 35 | geometry_msgs 36 | hector_uav_msgs 37 | std_srvs 38 | dynamic_reconfigure 39 | 40 | 41 | -------------------------------------------------------------------------------- /hector_quadrotor_model/.gitignore: -------------------------------------------------------------------------------- 1 | matlab/codegen/ 2 | -------------------------------------------------------------------------------- /hector_quadrotor_model/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_model 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | * make models more robust against irregular input values 11 | Especially on collisions with walls and obstacles Gazebo can output very high 12 | velocity values for some reason, which caused the propulsion and aerodynamic 13 | models to output infinite values or NaN as resulting forces and torques, with 14 | the consequence that Gazebo effectively stopped simulation and showed the quadrotor 15 | at the origin of the world frame. 16 | With this patch the simulation is much more stable in case of collisions or 17 | hard landings. 18 | * disabled quadratic term (CT2s) in propeller speed to performance factor J 19 | Because of this term the model output a non-zero thrust even if the propeller speed was zero. 20 | The quadratic term is due to drag and therefore covered in the aerodynamics model. 21 | * Contributors: Johannes Meyer 22 | 23 | 0.3.3 (2014-09-01) 24 | ------------------ 25 | * fixed target_link_libraries() line for the quadrotor_aerodynamics library 26 | * fixed missing cmake commands to link with boost and roscpp 27 | * always use newest available motor command in propulsion plugin 28 | * added cmake_modules dependency 29 | * Contributors: Johannes Meyer, Rasit Eskicioglu 30 | 31 | 0.3.2 (2014-03-30) 32 | ------------------ 33 | 34 | 0.3.1 (2013-12-26) 35 | ------------------ 36 | * fixed copy&paste error in quadrotor_aerodynamics.cpp 37 | * output drag forces in body coordinate system and added orientation input 38 | * fixed configuration namespace and plugin cleanup 39 | * added boolean return value for configure() methods 40 | * accept hector_uav_msgs/MotorCommand messages directly for the propulsion model/plugin 41 | * copied generated C code within the cpp classes to not require Matlab for compilation 42 | * fixed sign of aerodynamic forces 43 | * use precompiled codegen libraries if available 44 | * Contributors: Johannes Meyer 45 | 46 | 0.3.0 (2013-09-11) 47 | ------------------ 48 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 49 | -------------------------------------------------------------------------------- /hector_quadrotor_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_model) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS geometry_msgs hector_uav_msgs roscpp cmake_modules) 8 | include_directories(include ${catkin_INCLUDE_DIRS}) 9 | 10 | ## System dependencies are found with CMake's conventions 11 | find_package(Boost REQUIRED COMPONENTS thread) 12 | include_directories(${Boost_INCLUDE_DIRS}) 13 | 14 | find_package(Eigen REQUIRED) 15 | include_directories(${EIGEN_INCLUDE_DIRS}) 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | 23 | ################################### 24 | ## catkin specific configuration ## 25 | ################################### 26 | ## The catkin_package macro generates cmake config files for your package 27 | ## Declare things to be passed to dependent projects 28 | ## INCLUDE_DIRS: uncomment this if you package contains header files 29 | ## LIBRARIES: libraries you create in this project that dependent projects also need 30 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 31 | ## DEPENDS: system dependencies of this project that dependent projects also need 32 | catkin_package( 33 | INCLUDE_DIRS include 34 | LIBRARIES hector_quadrotor_propulsion hector_quadrotor_aerodynamics 35 | CATKIN_DEPENDS geometry_msgs hector_uav_msgs roscpp 36 | DEPENDS Boost 37 | ) 38 | 39 | ########### 40 | ## Build ## 41 | ########### 42 | 43 | add_library(hector_quadrotor_propulsion src/quadrotor_propulsion.cpp) 44 | add_dependencies(hector_quadrotor_propulsion hector_uav_msgs_generate_messages_cpp) 45 | target_link_libraries(hector_quadrotor_propulsion ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 46 | #target_link_libraries(hector_quadrotor_propulsion quadrotorPropulsion) 47 | 48 | add_library(hector_quadrotor_aerodynamics src/quadrotor_aerodynamics.cpp) 49 | target_link_libraries(hector_quadrotor_aerodynamics ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 50 | #target_link_libraries(hector_quadrotor_aerodynamics quadrotorDrag) 51 | 52 | ############# 53 | ## Install ## 54 | ############# 55 | 56 | # all install targets should use catkin DESTINATION variables 57 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 58 | 59 | install( 60 | TARGETS hector_quadrotor_propulsion hector_quadrotor_aerodynamics 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | ) 63 | 64 | ## Mark cpp header files for installation 65 | install(DIRECTORY include/${PROJECT_NAME}/ 66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | FILES_MATCHING PATTERN "*.h" 68 | PATTERN ".svn" EXCLUDE 69 | ) 70 | 71 | install(DIRECTORY param DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 72 | 73 | -------------------------------------------------------------------------------- /hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_aerodynamics.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef HECTOR_QUADROTOR_MODEL_QUADROTOR_AERODYNAMICS_H 30 | #define HECTOR_QUADROTOR_MODEL_QUADROTOR_AERODYNAMICS_H 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | 41 | namespace hector_quadrotor_model 42 | { 43 | 44 | class QuadrotorAerodynamics { 45 | public: 46 | QuadrotorAerodynamics(); 47 | ~QuadrotorAerodynamics(); 48 | 49 | bool configure(const ros::NodeHandle ¶m = ros::NodeHandle("~")); 50 | void reset(); 51 | void update(double dt); 52 | 53 | void setOrientation(const geometry_msgs::Quaternion& orientation); 54 | void setTwist(const geometry_msgs::Twist& twist); 55 | void setBodyTwist(const geometry_msgs::Twist& twist); 56 | void setWind(const geometry_msgs::Vector3& wind); 57 | 58 | const geometry_msgs::Wrench& getWrench() const { return wrench_; } 59 | 60 | void f(const double uin[6], double dt, double y[6]) const; 61 | 62 | private: 63 | geometry_msgs::Quaternion orientation_; 64 | geometry_msgs::Twist twist_; 65 | geometry_msgs::Vector3 wind_; 66 | 67 | geometry_msgs::Wrench wrench_; 68 | 69 | boost::mutex mutex_; 70 | 71 | class DragModel; 72 | DragModel *drag_model_; 73 | }; 74 | 75 | } 76 | 77 | #endif // HECTOR_QUADROTOR_MODEL_QUADROTOR_AERODYNAMICS_H 78 | -------------------------------------------------------------------------------- /hector_quadrotor_model/matlab/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_program(MATLAB NAMES matlab) 2 | 3 | if(MATLAB) 4 | message(STATUS "Using Matlab at ${MATLAB}") 5 | file(GLOB MFILES "*.m") 6 | 7 | include_directories(codegen/lib) 8 | 9 | add_custom_command( 10 | OUTPUT codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a codegen/lib/quadrotorDrag/quadrotorDrag.a 11 | COMMAND ${MATLAB} -nojvm -nodesktop -nosplash -r "compile_all; quit;" 12 | DEPENDS ${MFILES} 13 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 14 | VERBATIM 15 | ) 16 | 17 | # add_library(quadrotorPropulsion STATIC IMPORTED GLOBAL) 18 | # set_target_properties(quadrotorPropulsion PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) 19 | 20 | # workaround for cmake <2.8.8 21 | add_library(quadrotorPropulsion STATIC dummy.c) 22 | target_link_libraries(quadrotorPropulsion ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) 23 | 24 | # add_library(quadrotorDrag STATIC IMPORTED GLOBAL) 25 | # set_target_properties(quadrotorDrag PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) 26 | 27 | # workaround for cmake <2.8.8 28 | add_library(quadrotorDrag STATIC dummy.c) 29 | target_link_libraries(quadrotorDrag ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) 30 | 31 | else() 32 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) 33 | message(STATUS "Found precompiled hector_quadrotor propulsion model.") 34 | 35 | # add_library(quadrotorPropulsion STATIC IMPORTED GLOBAL) 36 | # set_target_properties(quadrotorPropulsion PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) 37 | 38 | # workaround for cmake <2.8.8 39 | add_library(quadrotorPropulsion STATIC dummy.c) 40 | target_link_libraries(quadrotorPropulsion ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) 41 | 42 | endif() 43 | 44 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) 45 | message(STATUS "Found precompiled hector_quadrotor drag model.") 46 | 47 | # add_library(quadrotorDrag STATIC IMPORTED GLOBAL) 48 | # set_target_properties(quadrotorDrag PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) 49 | 50 | # workaround for cmake <2.8.8 51 | add_library(quadrotorDrag STATIC dummy.c) 52 | target_link_libraries(quadrotorDrag ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) 53 | endif() 54 | 55 | if(NOT TARGET quadrotorPropulsion OR NOT TARGET quadrotorDrag) 56 | message(WARNING "Matlab not found. Cannot build hector_quadrotor_model libraries.") 57 | endif() 58 | endif(MATLAB) 59 | 60 | set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES ${CMAKE_CURRENT_SOURCE_DIR}/codegen) 61 | -------------------------------------------------------------------------------- /hector_quadrotor_model/matlab/compile_all.m: -------------------------------------------------------------------------------- 1 | rtw_config = coder.config('LIB'); 2 | rtw_config.TargetLang = 'C'; 3 | rtw_config.CodeReplacementLibrary = 'C89/C90 (ANSI)'; 4 | rtw_config.CCompilerOptimization = 'On'; 5 | 6 | codegen -config rtw_config -v quadrotorPropulsion motorspeed -o quadrotorPropulsion 7 | codegen -config rtw_config -v quadrotorDrag -o quadrotorDrag 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_model/matlab/dummy.c: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /hector_quadrotor_model/matlab/motorspeed.m: -------------------------------------------------------------------------------- 1 | function [M_e,I,xpred] = motorspeed(xin, uin, parameter, dt) 2 | %#eml 3 | 4 | assert(isa(xin,'double')); 5 | assert(all(size(xin) == 1)); 6 | 7 | assert(isa(uin,'double')); 8 | assert(all(size(uin) == [2 1])); 9 | 10 | assert(isa(parameter,'struct')); 11 | assert(isa(parameter.k_m,'double')); 12 | assert(isa(parameter.k_t,'double')); 13 | 14 | assert(isa(parameter.CT2s,'double')); 15 | assert(isa(parameter.CT1s,'double')); 16 | assert(isa(parameter.CT0s,'double')); 17 | 18 | assert(isa(parameter.Psi,'double')); 19 | assert(isa(parameter.J_M,'double')); 20 | assert(isa(parameter.R_A,'double')); 21 | 22 | assert(isa(parameter.alpha_m,'double')); 23 | assert(isa(parameter.beta_m,'double')); 24 | 25 | assert(isa(parameter.l_m,'double')); 26 | 27 | assert(isa(dt,'double')); 28 | assert(all(size(dt) == 1)); 29 | 30 | eml.cstructname(parameter,'PropulsionParameters'); 31 | 32 | % Identification of Roxxy2827-34 motor with 10x4.5 propeller 33 | Psi = parameter.Psi; 34 | J_M = parameter.J_M; 35 | R_A = parameter.R_A; 36 | k_m = parameter.k_m; 37 | k_t = parameter.k_t; 38 | alpha_m = parameter.alpha_m; 39 | beta_m = parameter.beta_m; 40 | 41 | % temporarily used Expressions 42 | U = uin(1); 43 | F_m = uin(2); 44 | omega_m = xin(1); 45 | 46 | M_m = k_t*F_m + k_m*omega_m; 47 | 48 | temp = (U*beta_m-Psi*omega_m)./(2*R_A); 49 | I = temp + sqrt(temp.^2 + (U*alpha_m./R_A)); 50 | M_e = Psi*I; % electrical torque motor 1-4 51 | 52 | % new version 53 | fx = 1/J_M.*(M_e - M_m); 54 | 55 | % old version 56 | % fx = (Psi/R_A*(U-Psi*omega_m) - M_m)/J_M; 57 | % A = -(Psi^2/R_A)/J_M; 58 | % B(1) = Psi/(J_M*R_A); 59 | % B(2) = -1/J_M; 60 | 61 | % system outputs. Use euler solver to predict next time step 62 | % predicted motor speed 63 | xpred = xin + dt*fx; 64 | % electric torque 65 | %y = [M_e I]; 66 | 67 | % system jacobian 68 | % A = 1 + dt*A; 69 | % input jacobian 70 | % B = A*B*dt; 71 | 72 | -------------------------------------------------------------------------------- /hector_quadrotor_model/matlab/parameter_QK_propulsion.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/hector_quadrotor/f29ce7512b2909a6c78237e61c31598f5270e016/hector_quadrotor_model/matlab/parameter_QK_propulsion.m -------------------------------------------------------------------------------- /hector_quadrotor_model/matlab/quadrotorDrag.m: -------------------------------------------------------------------------------- 1 | function y = quadrotorDrag(uin, parameter, dt) 2 | %#eml 3 | 4 | assert(isa(uin,'double')); 5 | assert(all(size(uin) == [6 1])); 6 | 7 | assert(isa(parameter,'struct')); 8 | assert(isa(parameter.C_wxy,'double')); 9 | assert(isa(parameter.C_wz,'double')); 10 | 11 | assert(isa(parameter.C_mxy,'double')); 12 | assert(isa(parameter.C_mz,'double')); 13 | 14 | 15 | assert(isa(dt,'double')); 16 | assert(all(size(dt) == 1)); 17 | 18 | eml.cstructname(parameter,'DragParameters'); 19 | 20 | % initialize vectors 21 | y = zeros(6,1); 22 | 23 | % Input variables 24 | u = uin(1); 25 | v = uin(2); 26 | w = uin(3); 27 | p = uin(4); 28 | q = uin(5); 29 | r = uin(6); 30 | 31 | % Constants 32 | C_wxy = parameter.C_wxy; 33 | C_wz = parameter.C_wz; 34 | C_mxy = parameter.C_mxy; 35 | C_mz = parameter.C_mz; 36 | 37 | % temporarily used vector 38 | absoluteVelocity = sqrt(u^2 + v^2 + w^2); 39 | absoluteAngularVelocity = sqrt(p^2 + q^2 + r^2); 40 | 41 | % system outputs 42 | % calculate drag force 43 | y(1) = C_wxy* absoluteVelocity*u; 44 | y(2) = C_wxy* absoluteVelocity*v; 45 | y(3) = C_wz * absoluteVelocity*w; 46 | 47 | % calculate draq torque 48 | y(4) = C_mxy* absoluteAngularVelocity*p; 49 | y(5) = C_mxy* absoluteAngularVelocity*q; 50 | y(6) = C_mz * absoluteAngularVelocity*r; 51 | -------------------------------------------------------------------------------- /hector_quadrotor_model/matlab/quadrotorPropulsion.m: -------------------------------------------------------------------------------- 1 | function [y,xpred] = quadrotorPropulsion(xin, uin, parameter, dt) 2 | %#eml 3 | 4 | assert(isa(xin,'double')); 5 | assert(all(size(xin) == [4 1])); 6 | 7 | assert(isa(uin,'double')); 8 | assert(all(size(uin) == [10 1])); 9 | 10 | assert(isa(parameter,'struct')); 11 | assert(isa(parameter.k_m,'double')); 12 | assert(isa(parameter.k_t,'double')); 13 | 14 | assert(isa(parameter.CT2s,'double')); 15 | assert(isa(parameter.CT1s,'double')); 16 | assert(isa(parameter.CT0s,'double')); 17 | 18 | assert(isa(parameter.Psi,'double')); 19 | assert(isa(parameter.J_M,'double')); 20 | assert(isa(parameter.R_A,'double')); 21 | 22 | assert(isa(parameter.alpha_m,'double')); 23 | assert(isa(parameter.beta_m,'double')); 24 | 25 | assert(isa(parameter.l_m,'double')); 26 | 27 | assert(isa(dt,'double')); 28 | assert(all(size(dt) == 1)); 29 | 30 | eml.cstructname(parameter,'PropulsionParameters'); 31 | 32 | % initialize vectors 33 | xpred = xin; % motorspeed 34 | v_1 = zeros(4,1); 35 | y = zeros(14,1); 36 | F_m = zeros(4,1); 37 | U = zeros(4,1); 38 | M_e = zeros(4,1); 39 | I = zeros(4,1); 40 | F = zeros(3,1); 41 | 42 | % Input variables 43 | u = uin(1); 44 | v = uin(2); 45 | w = uin(3); 46 | p = uin(4); 47 | q = uin(5); 48 | r = uin(6); 49 | U(1) = uin(7); 50 | U(2) = uin(8); 51 | U(3) = uin(9); 52 | U(4) = uin(10); 53 | 54 | % Constants 55 | CT0s = parameter.CT0s; 56 | CT1s = parameter.CT1s; 57 | CT2s = parameter.CT2s; 58 | k_t = parameter.k_t; 59 | l_m = parameter.l_m; 60 | Psi = parameter.Psi; 61 | 62 | v_1(1) = - w + l_m*q; 63 | v_1(2) = - w - l_m*p; 64 | v_1(3) = - w - l_m*q; 65 | v_1(4) = - w + l_m*p; 66 | 67 | % calculate thrust for all 4 rotors 68 | for i = 1:4 69 | % if the flow speed at infinity is negative 70 | if v_1(i) < 0 71 | F_m(i) = CT2s*v_1(i).^2 + CT1s*v_1(i).*xin(i) + CT0s*xin(i).^2; 72 | % if the flow speed at infinity is positive 73 | else 74 | F_m(i) = -CT2s*v_1(i).^2 + CT1s*v_1(i).*xin(i) + CT0s*xin(i).^2; 75 | end 76 | % sum up all rotor forces 77 | F(3) = F(3) + F_m(i) ; 78 | 79 | [M_e(i),I(i),xpred(i)] = motorspeed(xin(i), [U(i) F_m(i)], parameter, dt); 80 | end 81 | 82 | % System output, i.e. force and torque of quadrotor 83 | y(1) = F(1); 84 | y(2) = F(2); 85 | y(3) = F(3); 86 | 87 | % torque for rotating quadrocopter around x-axis is the mechanical torque 88 | y(4) = (F_m(4)-F_m(2))*l_m; 89 | % torque for rotating quadrocopter around y-axis is the mechanical torque 90 | y(5) = (F_m(1)-F_m(3))*l_m; 91 | % torque for rotating quadrocopter around z-axis is the electrical torque 92 | y(6) = (-M_e(1)-M_e(3)+M_e(2)+M_e(4)); 93 | 94 | % motor speeds (rad/s) 95 | y(7:10) = xpred(1:4); 96 | 97 | % motor current (A) 98 | y(11:14) = I(1:4); % M_e(1:4) / Psi; 99 | -------------------------------------------------------------------------------- /hector_quadrotor_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_model 3 | 0.3.5 4 | hector_quadrotor_model provides libraries that model several aspects of quadrotor dynamics. 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor_model 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | Alexander Sendobry 14 | 15 | 16 | catkin 17 | 18 | 19 | geometry_msgs 20 | hector_uav_msgs 21 | eigen 22 | roscpp 23 | cmake_modules 24 | boost 25 | 26 | 27 | geometry_msgs 28 | hector_uav_msgs 29 | roscpp 30 | boost 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /hector_quadrotor_model/param/quadrotor_aerodynamics.yaml: -------------------------------------------------------------------------------- 1 | quadrotor_aerodynamics: 2 | C_wxy: 0.12 3 | C_wz: 0.1 4 | C_mxy: 0.074156208000000 5 | C_mz: 0.050643264000000 -------------------------------------------------------------------------------- /hector_quadrotor_model/param/robbe_2827-34_epp1045.yaml: -------------------------------------------------------------------------------- 1 | quadrotor_propulsion: 2 | Psi: 0.007242179827506 3 | J_M: 2.573048063300000e-5 4 | R_A: 0.201084219222241 5 | k_t: 0.015336864714397 6 | k_m: -7.011631909766668e-5 7 | alpha_m: 0.104863758313889 8 | beta_m: 0.549262344777900 9 | 10 | #CT2s: -1.3077e-2 11 | CT2s: 0.0 12 | CT1s: -2.5224e-4 13 | CT0s: 1.538190483976698e-5 14 | 15 | l_m: 0.275 16 | -------------------------------------------------------------------------------- /hector_quadrotor_model/param/robbe_2827-34_epp1245.yaml: -------------------------------------------------------------------------------- 1 | quadrotor_propulsion: 2 | Psi: 0.007242179827506 3 | J_M: 4.142069415e-05 4 | R_A: 0.201084219222241 5 | k_t: 0.01732804081 6 | 7 | #CT2s: -2.3491553043010e-2 8 | CT2s: 0.0 9 | CT1s: -4.531245193522030e-04 10 | CT0s: 2.744543453e-05 11 | 12 | l_m: 0.275 13 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_pose_estimation 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | * added missing install rule for hector_quadrotor_pose_estimation_nodelets.xml 11 | Many thanks to Bernd Kast for pointing me to this issue. 12 | * update raw baro height as position.z component in sensor pose 13 | See https://github.com/tu-darmstadt-ros-pkg/hector_localization/commit/bd334f0e30c42fb5833fa8ffa249dfd737d43ddc. 14 | * updated package for the latest version of hector_pose_estimation 15 | * Geographic reference latitude and longitude set to 49.860246N 8.687077E (Lichtwiese). 16 | * Reenabled auto_elevation, auto_reference and auto_heading parameters for hector_pose_estimation. 17 | hector_pose_estimation will publish the world->nav transform depending on its reference pose. 18 | * added parameter file for hector_quadrotor_pose_estimation for a simulated quadrotor 19 | The parameter file disables the auto_elevation, auto_reference, auto_heading modes of hector_pose_estimation and sets the corresponding values 20 | manually with what is simulated in the gazebo sensor plugins. This simplifies the comparison of estimated poses with ground truth information. 21 | * shutdown the height subscriber (in favor of topic alitimeter) to not have two height updates 22 | * Contributors: Johannes Meyer 23 | 24 | 0.3.3 (2014-09-01) 25 | ------------------ 26 | 27 | 0.3.2 (2014-03-30) 28 | ------------------ 29 | 30 | 0.3.1 (2013-12-26) 31 | ------------------ 32 | 33 | 0.3.0 (2013-09-11) 34 | ------------------ 35 | * hector_quadrotor_pose_estimation: added cmake target dependency on hector_uav_msgs_generate_messages_cpp 36 | * hector_quadrotor: added package hector_quadrotor_pose_estimation 37 | * Contributors: Johannes Meyer 38 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/hector_quadrotor_pose_estimation_nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This nodelet implements a pose estimation filter which uses hector_uav_msgs/Altimeter messages as input for barometric height. 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/include/hector_quadrotor_pose_estimation/pose_estimation_node.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef HECTOR_QUADROTOR_POSE_ESTIMATION_NODE_H 30 | #define HECTOR_QUADROTOR_POSE_ESTIMATION_NODE_H 31 | 32 | #include 33 | #include 34 | 35 | namespace hector_quadrotor_pose_estimation { 36 | 37 | using namespace hector_pose_estimation; 38 | 39 | class QuadrotorPoseEstimationNode : public PoseEstimationNode { 40 | public: 41 | QuadrotorPoseEstimationNode(const SystemPtr& system = SystemPtr(), const StatePtr& state = StatePtr()); 42 | virtual ~QuadrotorPoseEstimationNode(); 43 | 44 | virtual bool init(); 45 | 46 | protected: 47 | void baroCallback(const hector_uav_msgs::AltimeterConstPtr& altimeter); 48 | 49 | private: 50 | ros::Subscriber baro_subscriber_; 51 | }; 52 | 53 | } // namespace hector_quadrotor_pose_estimation 54 | 55 | #endif // HECTOR_QUADROTOR_POSE_ESTIMATION_NODE_H 56 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_quadrotor_pose_estimation 4 | 0.3.5 5 | 6 | hector_quadrotor_pose_estimation provides a hector_pose_estimation node and nodelet specialized for hector_quadrotor. 7 | 8 | Johannes Meyer 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/hector_quadrotor_pose_estimation 13 | 14 | Johannes Meyer 15 | 16 | 17 | catkin 18 | 19 | 20 | hector_pose_estimation 21 | hector_uav_msgs 22 | 23 | 24 | hector_pose_estimation 25 | hector_uav_msgs 26 | nodelet 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/params/simulation.yaml: -------------------------------------------------------------------------------- 1 | reference_altitude: 0.0 2 | reference_heading: 0.0 3 | reference_latitude: 49.860246 4 | reference_longitude: 8.687077 5 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/src/main.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #include 30 | 31 | int main(int argc, char **argv) { 32 | ros::init(argc, argv, "pose_estimation"); 33 | hector_quadrotor_pose_estimation::QuadrotorPoseEstimationNode node; 34 | if (!node.init()) return 1; 35 | 36 | ros::spin(); 37 | 38 | node.cleanup(); 39 | return 0; 40 | } 41 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/src/pose_estimation_node.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #include 30 | #include 31 | 32 | namespace hector_quadrotor_pose_estimation { 33 | 34 | QuadrotorPoseEstimationNode::QuadrotorPoseEstimationNode(const SystemPtr& system, const StatePtr& state) 35 | : PoseEstimationNode(system, state) 36 | { 37 | pose_estimation_->addMeasurement(new Baro("baro")); 38 | } 39 | 40 | QuadrotorPoseEstimationNode::~QuadrotorPoseEstimationNode() 41 | { 42 | } 43 | 44 | bool QuadrotorPoseEstimationNode::init() { 45 | if (!PoseEstimationNode::init()) return false; 46 | baro_subscriber_ = getNodeHandle().subscribe("altimeter", 10, &QuadrotorPoseEstimationNode::baroCallback, this); 47 | height_subscriber_.shutdown(); 48 | return true; 49 | } 50 | 51 | void QuadrotorPoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& altimeter) { 52 | pose_estimation_->getMeasurement("baro")->add(Baro::Update(altimeter->pressure, altimeter->qnh)); 53 | 54 | if (sensor_pose_publisher_) { 55 | boost::shared_ptr baro = boost::static_pointer_cast(pose_estimation_->getMeasurement("baro")); 56 | sensor_pose_.pose.position.z = baro->getModel()->getAltitude(Baro::Update(altimeter->pressure, altimeter->qnh)) - baro->getElevation(); 57 | } 58 | } 59 | 60 | } // namespace hector_quadrotor_pose_estimation 61 | -------------------------------------------------------------------------------- /hector_quadrotor_pose_estimation/src/pose_estimation_nodelet.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #include 30 | #include 31 | 32 | namespace hector_quadrotor_pose_estimation { 33 | 34 | class QuadrotorPoseEstimationNodelet : public QuadrotorPoseEstimationNode, public nodelet::Nodelet 35 | { 36 | public: 37 | QuadrotorPoseEstimationNodelet(const SystemPtr& system = SystemPtr()) 38 | : QuadrotorPoseEstimationNode(system) 39 | {} 40 | 41 | private: 42 | void onInit() { 43 | QuadrotorPoseEstimationNode::init(); 44 | } 45 | 46 | void onReset() { 47 | QuadrotorPoseEstimationNode::reset(); 48 | } 49 | 50 | void onCleanup() { 51 | QuadrotorPoseEstimationNode::cleanup(); 52 | } 53 | }; 54 | 55 | } // namespace hector_quadrotor_pose_estimation 56 | 57 | #include 58 | PLUGINLIB_EXPORT_CLASS(hector_quadrotor_pose_estimation::QuadrotorPoseEstimationNodelet, nodelet::Nodelet) 59 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_teleop 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | * Add optional parameter to set the joystick device 11 | * Contributors: whoenig 12 | 13 | 0.3.3 (2014-09-01) 14 | ------------------ 15 | * added run dependency to joy package (joystick drivers) 16 | * Contributors: Johannes Meyer 17 | 18 | 0.3.2 (2014-03-30) 19 | ------------------ 20 | 21 | 0.3.1 (2013-12-26) 22 | ------------------ 23 | * added slow button (btn 6 on Logitech Gamepad) 24 | * Contributors: Johannes Meyer 25 | 26 | 0.3.0 (2013-09-11) 27 | ------------------ 28 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 29 | * decreased z_velocity_max to 2.0 m/s in logitech_gamepad.launch 30 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/launch/draw_circle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/launch/draw_rect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/launch/hovering.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/launch/logitech_gamepad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/launch/xbox_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_teleop 3 | 0.3.5 4 | hector_quadrotor_teleop enables quadrotor flying with a joystick by 5 | processing joy/Joy messages and translating them to geometry_msgs/Twist. 6 | Johannes Meyer 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/hector_quadrotor_teleop 11 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 12 | 13 | Johannes Meyer 14 | 15 | 16 | catkin 17 | 18 | 19 | roscpp 20 | sensor_msgs 21 | geometry_msgs 22 | hector_uav_msgs 23 | 24 | 25 | roscpp 26 | sensor_msgs 27 | geometry_msgs 28 | hector_uav_msgs 29 | joy 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/src/draw_circle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | using namespace Eigen; 9 | 10 | Matrix3d Rnb; 11 | Vector3d tnb; 12 | 13 | ros::Time star; 14 | 15 | int target_height; 16 | int velocity; 17 | int radius; 18 | 19 | void generate_path(double t, Vector3d& p, Vector3d& v) 20 | { 21 | double theta = velocity*t/radius; 22 | p(0) = radius*cos(theta); 23 | p(1) = radius*sin(theta); 24 | p(2) = target_height; 25 | v(0) = -velocity*sin(theta); 26 | v(1) = velocity*cos(theta); 27 | } 28 | 29 | Vector3d generate_vel_cmd(Vector3d p, Vector3d p_target, Vector3d v_target) 30 | { 31 | double k = 0.1; 32 | Vector3d v_cmd = k*(p_target - p) + v_target; 33 | return v_cmd; 34 | } 35 | 36 | int main(int argc, char** argv) 37 | { 38 | 39 | ros::init(argc, argv, "quadrotor_draw_circle"); 40 | ros::NodeHandle nh; 41 | nh.param("height", target_height, 3); 42 | nh.param("radius", radius, 4); 43 | nh.param("velocity", velocity, 4); 44 | cout << "radius: " << radius << "\n" 45 | << "height: " << target_height << "\n" 46 | << "velocity: " << velocity << endl; 47 | 48 | //cmd_vel_mux/input/teleop 49 | ros::Publisher twist_pub = nh.advertise("cmd_vel", 1); 50 | 51 | 52 | ros::Rate loop_rate(50); 53 | ros::Time start = ros::Time::now(); 54 | tf::TransformListener listener; 55 | 56 | while(ros::ok()) 57 | { 58 | 59 | Vector3d pt, vt; 60 | 61 | tf::StampedTransform transform; 62 | try{ 63 | //targe_frame <- source frame 64 | listener.waitForTransform("/world", "/base_link", start, ros::Duration(1.0)); 65 | listener.lookupTransform("/world", "/base_link", 66 | start, transform); 67 | } 68 | catch (tf::TransformException &ex) 69 | { 70 | ROS_ERROR("%s",ex.what()); 71 | ros::Duration(1.0).sleep(); 72 | continue; 73 | } 74 | Vector3d p; 75 | Quaterniond q; 76 | p(0) = transform.getOrigin().x(); 77 | p(1) = transform.getOrigin().y(); 78 | p(2) = transform.getOrigin().z(); 79 | q.w() = transform.getRotation().w(); 80 | q.x() = transform.getRotation().x(); 81 | q.y() = transform.getRotation().y(); 82 | q.z() = transform.getRotation().z(); 83 | Matrix3d Rnb = q.toRotationMatrix(); 84 | 85 | double t = (ros::Time::now() - start).toSec(); 86 | generate_path(t, pt, vt); 87 | Vector3d vel_cmd_n = generate_vel_cmd(p, pt, vt); 88 | //vel in world to body 89 | Vector3d vel_cmd = Rnb.transpose()*vel_cmd_n;//world to body 90 | cout << "vel_cmd: " << vel_cmd.transpose() << endl; 91 | geometry_msgs::Twist twist; 92 | twist.linear.x = vel_cmd(0); 93 | twist.linear.y = vel_cmd(1); 94 | twist.linear.z = vel_cmd(2); 95 | twist_pub.publish(twist); 96 | loop_rate.sleep(); 97 | } 98 | 99 | ros::spin(); 100 | return 0; 101 | } 102 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/src/quadrotor_teleop_random.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | // 8 | enum State 9 | { 10 | TAKEOFF,//linear.z > for takeoff 11 | FORWARD, 12 | TURN, 13 | }; 14 | State g_state = TAKEOFF; 15 | float linear_x = 0.0; 16 | float linear_y = 0.0; 17 | float linear_z = 0.0; 18 | float angular_z = 0.0; 19 | 20 | void toogleState() 21 | { 22 | if(g_state == TAKEOFF) 23 | { 24 | g_state = FORWARD; 25 | linear_x = (rand()%100 - 50)/100.0; 26 | linear_y = 0;//rand()%100/100.0; 27 | linear_z = 0;//(rand()%100 - 50)/1000.0; 28 | 29 | }else if(g_state == FORWARD) 30 | { 31 | g_state = TURN; 32 | angular_z = rand()%100/50.0; 33 | 34 | }else if(g_state == TURN) 35 | { 36 | g_state = FORWARD; 37 | linear_x = rand()%100/50.0; 38 | } 39 | } 40 | 41 | void commandTurtle(ros::Publisher twist_pub, float linear, float angular) 42 | { 43 | geometry_msgs::Twist twist; 44 | twist.linear.x = linear; 45 | twist.angular.z = angular; 46 | cout << "vel: " << linear << endl; 47 | twist_pub.publish(twist); 48 | } 49 | 50 | void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub) 51 | { 52 | static int count = 0; 53 | count++; 54 | 55 | if(g_state == TAKEOFF) 56 | { 57 | 58 | geometry_msgs::Twist twist; 59 | linear_z = rand()%100/100.0; 60 | twist.linear.z = linear_z; 61 | twist_pub.publish(twist); 62 | 63 | if(count % 100 == 0) 64 | { 65 | toogleState(); 66 | return; 67 | } 68 | 69 | }else if(g_state == FORWARD) 70 | { 71 | geometry_msgs::Twist twist; 72 | twist.linear.x = linear_x; 73 | twist.linear.y = linear_y; 74 | twist.linear.z = linear_z; 75 | twist_pub.publish(twist); 76 | 77 | if(count % 200 == 0) 78 | { 79 | toogleState(); 80 | return; 81 | } 82 | }else if(g_state == TURN) 83 | { 84 | geometry_msgs::Twist twist; 85 | twist.angular.z = angular_z; 86 | 87 | twist_pub.publish(twist); 88 | if(count % 100 == 0) 89 | { 90 | toogleState(); 91 | return; 92 | } 93 | } 94 | } 95 | 96 | int main(int argc, char** argv) 97 | { 98 | ros::init(argc, argv, "quadrotor_teleop_random"); 99 | ros::NodeHandle nh; 100 | //cmd_vel_mux/input/teleop 101 | ros::Publisher twist_pub = nh.advertise("cmd_vel", 1); 102 | 103 | ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twist_pub)); 104 | srand(time(0)); 105 | ros::spin(); 106 | 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /hector_quadrotor_teleop/src/spinning_lidar_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | int main(int argc, char** argv){ 6 | ros::init(argc, argv, "spinning_lidar_control"); 7 | ros::NodeHandle node; 8 | ros::Rate loop_rate(50); 9 | static tf::TransformBroadcaster br; 10 | tf::Transform transform; 11 | ros::Time start = ros::Time::now(); 12 | 13 | while(ros::ok()) 14 | { 15 | tf::Quaternion q; 16 | double t = (ros::Time::now() - start).toSec(); 17 | double psy = t*0.5;//0.1 rad/s 18 | q.setRPY(0, 0, psy); 19 | transform.setRotation(q); 20 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "spinning_hokuyo_spinning_lidar_root_link", "spinning_hokuyo_spinning_lidar_spin_link")); 21 | loop_rate.sleep(); 22 | } 23 | return 0; 24 | }; 25 | 26 | -------------------------------------------------------------------------------- /hector_uav_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_uav_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | * updated value type in RawImu message to int16 14 | * added new ControllerState status constant FLYING 15 | * added CLIMBRATE and TURNRATE constants to ControllerState message 16 | * Contributors: Johannes Meyer 17 | 18 | 0.3.2 (2014-03-30) 19 | ------------------ 20 | 21 | 0.3.1 (2013-12-26) 22 | ------------------ 23 | 24 | 0.3.0 (2013-09-11) 25 | ------------------ 26 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 27 | -------------------------------------------------------------------------------- /hector_uav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_uav_msgs) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED message_generation std_msgs) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | add_message_files( 24 | FILES 25 | Altimeter.msg 26 | AttitudeCommand.msg 27 | Compass.msg 28 | ControllerState.msg 29 | HeadingCommand.msg 30 | HeightCommand.msg 31 | MotorCommand.msg 32 | MotorPWM.msg 33 | MotorStatus.msg 34 | PositionXYCommand.msg 35 | RawImu.msg 36 | RawMagnetic.msg 37 | RawRC.msg 38 | RC.msg 39 | RuddersCommand.msg 40 | ServoCommand.msg 41 | Supply.msg 42 | ThrustCommand.msg 43 | VelocityXYCommand.msg 44 | VelocityZCommand.msg 45 | YawrateCommand.msg 46 | ) 47 | 48 | ## Generate services in the 'srv' folder 49 | # add_service_files( 50 | # FILES 51 | # Service1.srv 52 | # Service2.srv 53 | # ) 54 | 55 | ## Generate added messages and services with any dependencies listed here 56 | generate_messages( 57 | DEPENDENCIES 58 | std_msgs 59 | ) 60 | 61 | ################################### 62 | ## catkin specific configuration ## 63 | ################################### 64 | ## The catkin_package macro generates cmake config files for your package 65 | ## Declare things to be passed to dependent projects 66 | ## INCLUDE_DIRS: uncomment this if you package contains header files 67 | ## LIBRARIES: libraries you create in this project that dependent projects also need 68 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 69 | ## DEPENDS: system dependencies of this project that dependent projects also need 70 | catkin_package( 71 | INCLUDE_DIRS include 72 | # LIBRARIES hector_uav_msgs 73 | CATKIN_DEPENDS message_runtime std_msgs 74 | # DEPENDS system_lib 75 | ) 76 | 77 | ############# 78 | ## Install ## 79 | ############# 80 | 81 | # all install targets should use catkin DESTINATION variables 82 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 83 | 84 | ## Mark cpp header files for installation 85 | install(DIRECTORY include/${PROJECT_NAME}/ 86 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 87 | FILES_MATCHING PATTERN "*.h" 88 | PATTERN ".svn" EXCLUDE 89 | ) 90 | 91 | -------------------------------------------------------------------------------- /hector_uav_msgs/include/hector_uav_msgs/Altimeter/pressure_height.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2013, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef HECTOR_UAV_MSGS_ALTIMETER_PRESSURE_HEIGHT_H 30 | #define HECTOR_UAV_MSGS_ALTIMETER_PRESSURE_HEIGHT_H 31 | 32 | #include 33 | #include 34 | 35 | namespace hector_uav_msgs { 36 | 37 | static const Altimeter::_qnh_type STANDARD_PRESSURE = 1013.25; 38 | 39 | static inline Altimeter::_altitude_type altitudeFromPressure(Altimeter::_pressure_type pressure, Altimeter::_qnh_type qnh = STANDARD_PRESSURE) { 40 | return 288.15 / 0.0065 * (1.0 - pow(pressure / qnh, 1.0/5.255)); 41 | } 42 | 43 | static inline Altimeter::_pressure_type pressureFromAltitude(Altimeter::_altitude_type altitude, Altimeter::_qnh_type qnh = STANDARD_PRESSURE) { 44 | return qnh * pow(1.0 - (0.0065 * altitude) / 288.15, 5.255); 45 | } 46 | 47 | static inline Altimeter& altitudeFromPressure(Altimeter& altimeter) { 48 | if (altimeter.qnh == 0.0) altimeter.qnh = STANDARD_PRESSURE; 49 | altimeter.altitude = altitudeFromPressure(altimeter.pressure, altimeter.qnh); 50 | return altimeter; 51 | } 52 | 53 | static inline Altimeter& pressureFromAltitude(Altimeter& altimeter) { 54 | if (altimeter.qnh == 0.0) altimeter.qnh = STANDARD_PRESSURE; 55 | altimeter.pressure = pressureFromAltitude(altimeter.altitude, altimeter.qnh); 56 | return altimeter; 57 | } 58 | 59 | } // namespace hector_uav_msgs 60 | 61 | #endif // HECTOR_UAV_MSGS_ALTIMETER_PRESSURE_HEIGHT_H 62 | -------------------------------------------------------------------------------- /hector_uav_msgs/include/hector_uav_msgs/ControlSource.h: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | #ifndef HECTOR_UAV_MSGS_CONTROLSOURCE_H 30 | #define HECTOR_UAV_MSGS_CONTROLSOURCE_H 31 | 32 | namespace hector_uav_msgs 33 | { 34 | typedef uint8_t ControlSource; 35 | 36 | enum { 37 | CONTROL_AUTONOMOUS = 0, 38 | CONTROL_REMOTE = 1, 39 | CONTROL_JOYSTICK = 2 40 | }; 41 | 42 | template 43 | static inline InStream& operator>>(InStream& in, ControlSource& value) { 44 | int temp; 45 | in >> temp; 46 | value = static_cast(temp); 47 | return in; 48 | } 49 | 50 | template 51 | static inline OutStream& operator<<(OutStream& out, const ControlSource& value) { 52 | return out << static_cast(value); 53 | } 54 | } 55 | 56 | #endif // HECTOR_UAV_MSGS_CONTROLSOURCE_H 57 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/Altimeter.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 altitude 3 | float32 pressure 4 | float32 qnh 5 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/AttitudeCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 roll 3 | float32 pitch 4 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/Compass.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 magnetic_heading 3 | float32 declination 4 | 5 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/ControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 source 3 | 4 | uint8 mode 5 | uint8 MOTORS = 1 6 | uint8 ATTITUDE = 2 7 | uint8 VELOCITY = 4 8 | uint8 POSITION = 8 9 | uint8 TURNRATE = 16 10 | uint8 HEADING = 32 11 | uint8 CLIMBRATE = 64 12 | uint8 HEIGHT = 128 13 | 14 | uint8 state 15 | uint8 MOTORS_RUNNING = 1 16 | uint8 FLYING = 2 17 | uint8 AIRBORNE = 4 18 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/HeadingCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 heading 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/HeightCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 height 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/MotorCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32[] force 3 | float32[] torque 4 | float32[] frequency 5 | float32[] voltage 6 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/MotorPWM.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8[] pwm 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/MotorStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | bool on 3 | bool running 4 | float32[] voltage 5 | float32[] frequency 6 | float32[] current 7 | 8 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/PositionXYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/RC.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint8 ROLL = 1 4 | uint8 PITCH = 2 5 | uint8 YAW = 3 6 | uint8 STEER = 4 7 | uint8 HEIGHT = 5 8 | uint8 THRUST = 6 9 | uint8 BRAKE = 7 10 | 11 | uint8 status 12 | bool valid 13 | 14 | float32[] axis 15 | uint8[] axis_function 16 | 17 | int8[] swit 18 | uint8[] swit_function 19 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/RawImu.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int16[3] angular_velocity 3 | int16[3] linear_acceleration 4 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/RawMagnetic.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[3] channel 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/RawRC.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 status 3 | uint16[] channel 4 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/RuddersCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 aileron 3 | float32 elevator 4 | float32 rudder 5 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/ServoCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16[] value 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/Supply.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32[] voltage 3 | float32[] current 4 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/ThrustCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 thrust 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/VelocityXYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/VelocityZCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 z 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/msg/YawrateCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 turnrate 3 | -------------------------------------------------------------------------------- /hector_uav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_uav_msgs 3 | 0.3.5 4 | hector_uav_msgs is a message package that contains messages for UAV controller inputs and outputs and some sensor readings not covered by sensor_msgs. 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_uav_msgs 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | 14 | 15 | catkin 16 | 17 | 18 | message_generation 19 | std_msgs 20 | 21 | 22 | message_runtime 23 | std_msgs 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /irobot_teleop/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_teleop 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | * Add optional parameter to set the joystick device 11 | * Contributors: whoenig 12 | 13 | 0.3.3 (2014-09-01) 14 | ------------------ 15 | * added run dependency to joy package (joystick drivers) 16 | * Contributors: Johannes Meyer 17 | 18 | 0.3.2 (2014-03-30) 19 | ------------------ 20 | 21 | 0.3.1 (2013-12-26) 22 | ------------------ 23 | * added slow button (btn 6 on Logitech Gamepad) 24 | * Contributors: Johannes Meyer 25 | 26 | 0.3.0 (2013-09-11) 27 | ------------------ 28 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 29 | * decreased z_velocity_max to 2.0 m/s in logitech_gamepad.launch 30 | -------------------------------------------------------------------------------- /irobot_teleop/launch/logitech_gamepad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /irobot_teleop/launch/xbox_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /irobot_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | irobot_teleop 3 | 0.3.5 4 | irobot_teleop enables quadrotor flying with a joystick by 5 | processing joy/Joy messages and translating them to geometry_msgs/Twist. 6 | Johannes Meyer 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/irobot_teleop 11 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 12 | 13 | Johannes Meyer 14 | 15 | 16 | catkin 17 | 18 | 19 | roscpp 20 | sensor_msgs 21 | geometry_msgs 22 | hector_uav_msgs 23 | 24 | 25 | roscpp 26 | sensor_msgs 27 | geometry_msgs 28 | hector_uav_msgs 29 | joy 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /irobot_teleop/src/irobot_random_walk.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace std; 7 | 8 | const int nrobot = 10; 9 | 10 | 11 | enum State 12 | { 13 | FORWARD, 14 | TURN, 15 | }; 16 | 17 | void toogleState(State& state, double & linear, double & angular) 18 | { 19 | if(state == FORWARD) 20 | { 21 | state = TURN; 22 | angular = rand()%100/50.0; 23 | }else if(state == TURN) 24 | { 25 | state = FORWARD; 26 | linear = rand()%100/50.0; 27 | } 28 | } 29 | 30 | void timerCallback(const ros::TimerEvent&, ros::Publisher* twists_pub) 31 | { 32 | static int count = 0; 33 | static State states[nrobot] = {FORWARD}; 34 | static double linears[nrobot] = {0}; 35 | static double angulars[nrobot] = {0}; 36 | 37 | for(int i = 0; i < nrobot; i++) 38 | { 39 | if(states[i] == FORWARD) 40 | { 41 | geometry_msgs::Twist twist; 42 | twist.linear.x = linears[i]; 43 | twist.linear.y = 0; 44 | twist.linear.z = 0; 45 | twists_pub[i].publish(twist); 46 | 47 | if(count % 500 == 0) 48 | { 49 | toogleState(states[i], linears[i], angulars[i]); 50 | } 51 | }else if(states[i] == TURN) 52 | { 53 | geometry_msgs::Twist twist; 54 | twist.angular.z = angulars[i]; 55 | twists_pub[i].publish(twist); 56 | if(count % 200 == 0) 57 | { 58 | toogleState(states[i], linears[i], angulars[i]); 59 | } 60 | } 61 | } 62 | 63 | count++; 64 | } 65 | 66 | int main(int argc, char** argv) 67 | { 68 | ros::init(argc, argv, "irobot_teleop_random"); 69 | ros::NodeHandle nh; 70 | 71 | 72 | ros::Publisher twists_pub[nrobot]; 73 | for(int i = 0; i < nrobot; i++) 74 | { 75 | string topic = string("irobot") + std::to_string(i) + "/cmd_vel"; 76 | twists_pub[i] = nh.advertise(topic, 1); 77 | } 78 | 79 | ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twists_pub)); 80 | srand(time(0)); 81 | ros::spin(); 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /tutorials.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: indigo-devel} 2 | - git: {local-name: hector_slam, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_slam.git', version: catkin} 3 | - git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin} 4 | - git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: indigo-devel} 5 | - git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: indigo-devel} 6 | --------------------------------------------------------------------------------