From e30c14bbc8edfea338fbe7167c478953c2bb0b2f Mon Sep 17 00:00:00 2001 From: XunMeng2017 Date: Wed, 6 Mar 2019 14:34:44 +0800 Subject: [PATCH] submit hector_quadrotor and rosflight_ws --- .../hector_quadrotor.rosinstall | 4 + .../hector_quadrotor/CHANGELOG.rst | 26 + .../hector_quadrotor/CMakeLists.txt | 4 + .../hector_quadrotor/package.xml | 30 + .../hector_quadrotor_controller/CHANGELOG.rst | 46 ++ .../CMakeLists.txt | 78 +++ .../hector_quadrotor_controller/handles.h | 536 ++++++++++++++++++ .../include/hector_quadrotor_controller/pid.h | 45 ++ .../quadrotor_interface.h | 143 +++++ .../launch/controller.launch | 7 + .../hector_quadrotor_controller/package.xml | 42 ++ .../params/controller.yaml | 50 ++ .../hector_quadrotor_controller/plugin.xml | 23 + .../src/motor_controller.cpp | 194 +++++++ .../hector_quadrotor_controller/src/pid.cpp | 152 +++++ .../src/pose_controller.cpp | 205 +++++++ .../src/quadrotor_interface.cpp | 195 +++++++ .../src/twist_controller (复件).cpp | 328 +++++++++++ .../src/twist_controller.cpp | 408 +++++++++++++ .../CHANGELOG.rst | 20 + .../CMakeLists.txt | 122 ++++ .../quadrotor_hardware_gazebo.h | 109 ++++ .../package.xml | 24 + .../quadrotor_controller_gazebo.xml | 7 + .../src/quadrotor_hardware_gazebo.cpp | 275 +++++++++ .../hector_quadrotor_demo/CHANGELOG.rst | 25 + .../hector_quadrotor_demo/CMakeLists.txt | 67 +++ .../launch/indoor_slam_gazebo.launch | 26 + .../launch/outdoor_flight_gazebo.launch | 16 + .../hector_quadrotor_demo/package.xml | 28 + .../rviz_cfg/indoor_slam.rviz | 207 +++++++ .../rviz_cfg/outdoor_flight.rviz | 192 +++++++ .../CHANGELOG.rst | 22 + .../CMakeLists.txt | 68 +++ .../launch/xacrodisplay_quadrotor_base.launch | 9 + .../meshes/box.gazebo.xacro | 6 + .../meshes/box.urdf.xacro | 12 + .../meshes/box_base.urdf.xacro | 61 ++ .../quadrotor/blender/quadrotor_base.blend | Bin 0 -> 385148 bytes .../quadrotor/quadrotor_base (复件).dae | 238 ++++++++ .../meshes/quadrotor/quadrotor_base.dae | 229 ++++++++ .../meshes/quadrotor/quadrotor_base.stl | Bin 0 -> 27084 bytes .../hector_quadrotor_description/package.xml | 25 + .../urdf/box.gazebo.xacro | 7 + .../urdf/box.urdf.xacro | 14 + .../urdf/box_base.urdf.xacro | 32 ++ .../urdf/my_robot.urdf | 29 + .../urdf/quadrotor.gazebo.xacro | 7 + .../urdf/quadrotor.urdf.xacro | 14 + .../urdf/quadrotor1.gazebo.xacro | 7 + .../urdf/quadrotor1.urdf.xacro | 14 + .../urdf/quadrotor1_base.urdf.xacro | 35 ++ .../urdf/quadrotor_base.urdf.1.xacro | 39 ++ .../urdf/quadrotor_base.urdf.xacro | 64 +++ .../urdf/quadrotor_downward_cam.gazebo.xacro | 8 + .../urdf/quadrotor_downward_cam.urdf.xacro | 20 + .../quadrotor_downward_nocam.gazebo.xacro | 8 + .../urdf/quadrotor_downward_nocam.urdf.xacro | 20 + .../quadrotor_hokuyo_utm30lx.gazebo.xacro | 7 + .../urdf/quadrotor_hokuyo_utm30lx.urdf.xacro | 26 + .../urdf/quadrotor_with_asus.gazebo.xacro | 7 + .../urdf/quadrotor_with_asus.urdf.xacro | 20 + ...with_asus_with_hokuyo_utm30lx.gazebo.xacro | 7 + ...r_with_asus_with_hokuyo_utm30lx.urdf.xacro | 26 + .../urdf/quadrotor_with_cam.gazebo.xacro | 7 + .../urdf/quadrotor_with_cam.urdf.xacro | 20 + .../urdf/quadrotor_with_cam1.gazebo.xacro | 7 + .../urdf/quadrotor_with_cam1.urdf.xacro | 20 + .../urdf/quadrotor_with_kinect.gazebo.xacro | 7 + .../urdf/quadrotor_with_kinect.urdf.xacro | 20 + .../urdf/test_robot.gv | 14 + .../urdf/test_robot.pdf | Bin 0 -> 13979 bytes .../hector_quadrotor_gazebo/.gitignore | 1 + .../hector_quadrotor_gazebo/CHANGELOG.rst | 55 ++ .../hector_quadrotor_gazebo/CMakeLists.txt | 74 +++ .../hector_quadrotor_gazebo/launch/a.out | Bin 0 -> 14167 bytes .../launch/cxf_myspawn.launch | 204 +++++++ .../launch/generate.cpp | 35 ++ .../launch/myspawn.launch | 204 +++++++ .../launch/nine_quadrotor_empty_world.launch | 24 + .../launch/quadrotor_empty_world.launch | 24 + .../launch/spawn_box.launch | 74 +++ .../launch/spawn_nine_quadrotors.launch | 97 ++++ .../spawn_quadrotor (另一个复件).launch | 74 +++ .../launch/spawn_quadrotor(backup).launch | 74 +++ .../launch/spawn_quadrotor(zenglei).launch | 78 +++ .../launch/spawn_quadrotor.launch | 74 +++ .../launch/spawn_quadrotor_slam.launch | 31 + .../launch/spawn_quadrotor_split.launch | 78 +++ .../launch/spawn_quadrotor_with_asus.launch | 28 + ...pawn_quadrotor_with_asus_with_laser.launch | 28 + .../launch/spawn_quadrotor_with_cam.launch | 28 + .../spawn_quadrotor_with_downward_cam.launch | 28 + .../launch/spawn_quadrotor_with_kinect.launch | 28 + .../launch/spawn_quadrotor_with_laser.launch | 28 + .../launch/spawn_ten_quadrotors.launch | 107 ++++ .../launch/spawn_three_quadrotors.launch | 38 ++ .../launch/spawn_two_quadrotors.launch | 25 + .../launch/water_myspawn.launch | 204 +++++++ .../hector_quadrotor_gazebo/package.xml | 44 ++ .../urdf/CMakeLists.txt | 49 ++ .../urdf/quadrotor_aerodynamics.gazebo.xacro | 18 + .../urdf/quadrotor_controller.gazebo.xacro | 11 + .../urdf/quadrotor_plugins.gazebo.xacro.in | 16 + .../urdf/quadrotor_propulsion.gazebo.xacro | 22 + .../urdf/quadrotor_sensors.gazebo.xacro | 69 +++ .../CHANGELOG.rst | 50 ++ .../CMakeLists.txt | 122 ++++ .../gazebo_quadrotor_aerodynamics.h | 90 +++ .../gazebo_quadrotor_propulsion.h | 106 ++++ .../gazebo_quadrotor_simple_controller.h | 143 +++++ .../gazebo_ros_baro.h | 90 +++ .../package.xml | 40 ++ .../src/gazebo_quadrotor_aerodynamics.cpp | 236 ++++++++ .../src/gazebo_quadrotor_propulsion.cpp | 340 +++++++++++ .../gazebo_quadrotor_simple_controller.cpp | 426 ++++++++++++++ .../src/gazebo_ros_baro.cpp | 160 ++++++ .../hector_quadrotor_model/.gitignore | 1 + .../hector_quadrotor_model/CHANGELOG.rst | 48 ++ .../hector_quadrotor_model/CMakeLists.txt | 72 +++ .../include/hector_quadrotor_model/helpers.h | 177 ++++++ .../quadrotor_aerodynamics.h | 77 +++ .../quadrotor_propulsion.h | 99 ++++ .../matlab/CMakeLists.txt | 60 ++ .../matlab/compile_all.m | 8 + .../hector_quadrotor_model/matlab/dummy.c | 1 + .../matlab/motorspeed.m | 71 +++ .../matlab/parameter_QK_propulsion.m | 56 ++ .../matlab/quadrotorDrag.m | 50 ++ .../matlab/quadrotorPropulsion.m | 98 ++++ .../hector_quadrotor_model/package.xml | 34 ++ .../param/quadrotor_aerodynamics.yaml | 5 + .../param/robbe_2827-34_epp1045.yaml | 15 + .../param/robbe_2827-34_epp1245.yaml | 12 + .../src/matlab_helpers.h | 97 ++++ .../src/quadrotor_aerodynamics.cpp | 254 +++++++++ .../src/quadrotor_propulsion.cpp | 488 ++++++++++++++++ .../CHANGELOG.rst | 37 ++ .../CMakeLists.txt | 112 ++++ ...tor_quadrotor_pose_estimation_nodelets.xml | 8 + .../pose_estimation_node.h | 55 ++ .../package.xml | 34 ++ .../params/simulation.yaml | 4 + .../src/main.cpp | 40 ++ .../src/pose_estimation_node.cpp | 60 ++ .../src/pose_estimation_nodelet.cpp | 58 ++ .../hector_quadrotor_teleop/CHANGELOG.rst | 29 + .../hector_quadrotor_teleop/CMakeLists.txt | 89 +++ .../launch/logitech_gamepad.launch | 25 + .../launch/xbox_controller.launch | 19 + .../hector_quadrotor_teleop/package.xml | 33 ++ .../src/quadrotor_teleop.cpp | 215 +++++++ .../hector_uav_msgs/CHANGELOG.rst | 26 + .../hector_uav_msgs/CMakeLists.txt | 90 +++ .../Altimeter/pressure_height.h | 61 ++ .../include/hector_uav_msgs/ControlSource.h | 56 ++ .../include/hector_uav_msgs/RC/functions.h | 103 ++++ .../hector_uav_msgs/msg/Altimeter.msg | 4 + .../hector_uav_msgs/msg/AttitudeCommand.msg | 3 + .../hector_uav_msgs/msg/Compass.msg | 4 + .../hector_uav_msgs/msg/ControllerState.msg | 17 + .../hector_uav_msgs/msg/HeadingCommand.msg | 2 + .../hector_uav_msgs/msg/HeightCommand.msg | 2 + .../hector_uav_msgs/msg/MotorCommand.msg | 5 + .../hector_uav_msgs/msg/MotorPWM.msg | 2 + .../hector_uav_msgs/msg/MotorStatus.msg | 7 + .../hector_uav_msgs/msg/PositionXYCommand.msg | 3 + .../hector_uav_msgs/msg/RC.msg | 18 + .../hector_uav_msgs/msg/RawImu.msg | 3 + .../hector_uav_msgs/msg/RawMagnetic.msg | 2 + .../hector_uav_msgs/msg/RawRC.msg | 3 + .../hector_uav_msgs/msg/RuddersCommand.msg | 4 + .../hector_uav_msgs/msg/ServoCommand.msg | 2 + .../hector_uav_msgs/msg/Supply.msg | 3 + .../hector_uav_msgs/msg/ThrustCommand.msg | 2 + .../hector_uav_msgs/msg/VelocityXYCommand.msg | 3 + .../hector_uav_msgs/msg/VelocityZCommand.msg | 2 + .../hector_uav_msgs/msg/YawrateCommand.msg | 2 + .../hector_uav_msgs/package.xml | 27 + .../src/hector_quadrotor/tutorials.rosinstall | 5 + 180 files changed, 11587 insertions(+) create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor.rosinstall create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/handles.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/quadrotor_interface.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/launch/controller.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/params/controller.yaml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/plugin.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/motor_controller.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pid.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pose_controller.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/quadrotor_interface.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller (复件).cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller/quadrotor_hardware_gazebo.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/indoor_slam_gazebo.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_demo/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/indoor_slam.rviz create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/outdoor_flight.rviz create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/launch/xacrodisplay_quadrotor_base.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box_base.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/blender/quadrotor_base.blend create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base (复件).dae create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.dae create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box_base.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/my_robot.urdf create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1_base.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.1.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.urdf.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.gv create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.pdf create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/.gitignore create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/a.out create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/cxf_myspawn.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/generate.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/myspawn.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/nine_quadrotor_empty_world.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/quadrotor_empty_world.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_box.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_nine_quadrotors.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor (另一个复件).launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(backup).launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(zenglei).launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_slam.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_split.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus_with_laser.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_cam.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_kinect.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_laser.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_ten_quadrotors.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_three_quadrotors.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_two_quadrotors.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/water_myspawn.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_aerodynamics.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_controller.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro.in create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_propulsion.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_sensors.gazebo.xacro create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_aerodynamics.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_propulsion.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_simple_controller.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_aerodynamics.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_propulsion.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_simple_controller.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_ros_baro.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/.gitignore create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/helpers.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_aerodynamics.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_propulsion.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/compile_all.m create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/dummy.c create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/motorspeed.m create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/parameter_QK_propulsion.m create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorDrag.m create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorPropulsion.m create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/quadrotor_aerodynamics.yaml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1045.yaml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1245.yaml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/matlab_helpers.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_aerodynamics.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_propulsion.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/hector_quadrotor_pose_estimation_nodelets.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/include/hector_quadrotor_pose_estimation/pose_estimation_node.h create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/params/simulation.yaml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/main.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_node.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_nodelet.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/logitech_gamepad.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/xbox_controller.launch create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/package.xml create mode 100644 Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/src/quadrotor_teleop.cpp create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/CHANGELOG.rst create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/CMakeLists.txt create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/Altimeter/pressure_height.h create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/ControlSource.h create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/RC/functions.h create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Altimeter.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/AttitudeCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Compass.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ControllerState.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeadingCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeightCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorPWM.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorStatus.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/PositionXYCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RC.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawImu.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawMagnetic.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawRC.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RuddersCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ServoCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Supply.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ThrustCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityXYCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityZCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/YawrateCommand.msg create mode 100644 Flight_control/src/hector_quadrotor/hector_uav_msgs/package.xml create mode 100644 Flight_control/src/hector_quadrotor/tutorials.rosinstall diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor.rosinstall b/Flight_control/src/hector_quadrotor/hector_quadrotor.rosinstall new file mode 100644 index 0000000..93239bd --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor.rosinstall @@ -0,0 +1,4 @@ +- git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: indigo-devel} +- git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin} +- git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: indigo-devel} +- git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: indigo-devel} diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst new file mode 100644 index 0000000..b1514ca --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ + +0.3.3 (2014-09-01) +------------------ +* moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo +* Contributors: Johannes Meyer + +0.3.2 (2014-03-30) +------------------ +* added packages hector_quadrotor_controller and hector_quadrotor_controller_gazebo to meta-package +* Contributors: Johannes Meyer + +0.3.1 (2013-12-26) +------------------ + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt new file mode 100644 index 0000000..a4e0651 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor/package.xml new file mode 100644 index 0000000..6278fbd --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor/package.xml @@ -0,0 +1,30 @@ + + hector_quadrotor + 0.3.5 + hector_quadrotor contains packages related to modeling, control and simulation of quadrotor UAV systems + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + Stefan Kohlbrecher + + + catkin + + + + + + hector_quadrotor_controller + hector_quadrotor_description + hector_quadrotor_model + hector_quadrotor_teleop + hector_uav_msgs + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CHANGELOG.rst new file mode 100644 index 0000000..b502c46 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CHANGELOG.rst @@ -0,0 +1,46 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ +* updated angular/z controller parameters +* Remove redundant callback queue in twist_controller, use root_nh queue as per ros_control API +* Add controller timeout to allow faster shutdown of spawner +* Contributors: Johannes Meyer, Paul Bovbel + +0.3.4 (2015-02-22) +------------------ +* improved automatic landing detection and shutdown on rollovers +* slightly updated velocity controller limits and gains +* Contributors: Johannes Meyer + +0.3.3 (2014-09-01) +------------------ +* fixed some compiler warnings and missing return values +* increased integral gain for attitude stabilization (fix #12) +* make a copy of the root NodeHandle in all controllers + For some reason deconstructing the TwistController resulted in a pure virtual function call without this patch. +* Contributors: Johannes Meyer + +0.3.2 (2014-03-30) +------------------ +* Fix boost 1.53 issues + changed boost::shared_dynamic_cast to boost::dynamic_pointer_cast and + boost::shared_static_cast to boost::static_pointer_cast +* use a separate callback queue thread for the TwistController +* added optional twist limit in pose controller +* Contributors: Christopher Hrabia, Johannes Meyer + +0.3.1 (2013-12-26) +------------------ +* New controller implementation using ros_control +* Added pose controller +* Added motor controller that controls motor voltages from wrench commands +* Contributors: Johannes Meyer + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack +* Propulsion and aerodynamics models are in hector_quadrotor_model package now. +* Gazebo plugins are in hector_quadrotor_gazebo_plugins package now. diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CMakeLists.txt new file mode 100644 index 0000000..c3fca45 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_controller) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface) +include_directories(include ${catkin_INCLUDE_DIRS}) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES hector_quadrotor_controller + CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface + DEPENDS +) + +########### +## Build ## +########### + +add_library(hector_quadrotor_controller src/quadrotor_interface.cpp src/pid.cpp) +target_link_libraries(hector_quadrotor_controller ${catkin_LIBRARIES}) +add_dependencies(hector_quadrotor_controller hector_uav_msgs_generate_messages_cpp) + +add_library(hector_quadrotor_pose_controller src/pose_controller.cpp) +target_link_libraries(hector_quadrotor_pose_controller hector_quadrotor_controller) + +add_library(hector_quadrotor_twist_controller src/twist_controller.cpp) +target_link_libraries(hector_quadrotor_twist_controller hector_quadrotor_controller ${BOOST_LIBRARIES}) + +add_library(hector_quadrotor_motor_controller src/motor_controller.cpp) +target_link_libraries(hector_quadrotor_motor_controller hector_quadrotor_controller) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executables and/or libraries for installation +install(TARGETS hector_quadrotor_controller hector_quadrotor_pose_controller hector_quadrotor_twist_controller hector_quadrotor_motor_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY launch params DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES + plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/handles.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/handles.h new file mode 100644 index 0000000..641ad18 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/handles.h @@ -0,0 +1,536 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_CONTROLLER_HANDLES_H +#define HECTOR_QUADROTOR_CONTROLLER_HANDLES_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace hector_quadrotor_controller { + +class QuadrotorInterface; + +using geometry_msgs::Pose; +using geometry_msgs::Point; +using geometry_msgs::Quaternion; +using geometry_msgs::Twist; +using geometry_msgs::Vector3; +using geometry_msgs::Wrench; +using sensor_msgs::Imu; +using hector_uav_msgs::MotorStatus; +using hector_uav_msgs::MotorCommand; +using hector_uav_msgs::AttitudeCommand; +using hector_uav_msgs::YawrateCommand; +using hector_uav_msgs::ThrustCommand; + +template +class Handle_ +{ +public: + typedef T ValueType; + typedef Handle_ Base; + + Handle_(const std::string& name, const std::string& field = std::string()) : interface_(0), name_(name), field_(field), value_(0) {} + Handle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field), value_(0) {} + Handle_(QuadrotorInterface *interface, const ValueType *source, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field) { *this = source; } + virtual ~Handle_() {} + + virtual const std::string& getName() const { return name_; } + virtual const std::string& getField() const { return field_; } + + virtual bool connected() const { return get(); } + virtual void reset() { value_ = 0; } + + Derived& operator=(const ValueType *source) { value_ = source; return static_cast(*this); } + const ValueType *get() const { return value_; } + const ValueType &operator*() const { return *value_; } + +protected: + QuadrotorInterface *interface_; + const std::string name_; + const std::string field_; + const ValueType *value_; +}; + +class PoseHandle : public Handle_ +{ +public: + using Base::operator=; + + PoseHandle() : Base("pose") {} + PoseHandle(QuadrotorInterface *interface) : Base(interface, "pose") {} + PoseHandle(QuadrotorInterface *interface, const Pose *pose) : Base(interface, pose, "pose") {} + virtual ~PoseHandle() {} + + const ValueType& pose() const { return *get(); } + + void getEulerRPY(double &roll, double &pitch, double &yaw) const; + double getYaw() const; + Vector3 toBody(const Vector3& nav) const; + Vector3 fromBody(const Vector3& nav) const; +}; +typedef boost::shared_ptr PoseHandlePtr; + +class TwistHandle : public Handle_ +{ +public: + using Base::operator=; + + TwistHandle() : Base("twist") {} + TwistHandle(QuadrotorInterface *interface) : Base(interface, "twist") {} + TwistHandle(QuadrotorInterface *interface, const Twist *twist) : Base(interface, twist, "twist") {} + virtual ~TwistHandle() {} + + const ValueType& twist() const { return *get(); } +}; +typedef boost::shared_ptr TwistHandlePtr; + +class AccelerationHandle : public Handle_ +{ +public: + using Base::operator=; + + AccelerationHandle(QuadrotorInterface *interface, const Vector3 *acceleration) : Base(interface, acceleration, "acceleration") {} + virtual ~AccelerationHandle() {} + + const ValueType& acceleration() const { return *get(); } +}; +typedef boost::shared_ptr AccelerationHandlePtr; + +class StateHandle : public PoseHandle, public TwistHandle +{ +public: + StateHandle() {} + StateHandle(QuadrotorInterface *interface, const Pose *pose, const Twist *twist) : PoseHandle(interface, pose), TwistHandle(interface, twist) {} + virtual ~StateHandle() {} + + virtual bool connected() const { return PoseHandle::connected() && TwistHandle::connected(); } +}; +typedef boost::shared_ptr PoseHandlePtr; + +class ImuHandle : public Handle_ +{ +public: + using Base::operator=; + + ImuHandle() : Base("imu") {} + ImuHandle(QuadrotorInterface *interface, const Imu *imu) : Base(interface, imu, "imu") {} + virtual ~ImuHandle() {} + + const ValueType& imu() const { return *get(); } +}; +typedef boost::shared_ptr ImuHandlePtr; + +class MotorStatusHandle : public Handle_ +{ +public: + using Base::operator=; + + MotorStatusHandle() : Base("motor_status") {} + MotorStatusHandle(QuadrotorInterface *interface, const MotorStatus *motor_status) : Base(interface, motor_status, "motor_status") {} + virtual ~MotorStatusHandle() {} + + const ValueType& motorStatus() const { return *get(); } +}; +typedef boost::shared_ptr MotorStatusHandlePtr; + +class CommandHandle +{ +public: + CommandHandle() : interface_(0), new_value_(false) {} + CommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field) : interface_(interface), name_(name), field_(field), new_value_(false) {} + virtual ~CommandHandle() {} + + virtual const std::string& getName() const { return name_; } + virtual const std::string& getField() const { return field_; } + virtual bool connected() const = 0; + virtual void reset() {} + + void *get() const { return 0; } + + bool enabled(); + bool start(); + void stop(); + void disconnect(); + + template T* ownData(T* data) { my_.reset(data); return data; } + + template bool connectFrom(const Derived& output) { + Derived *me = dynamic_cast(this); + if (!me) return false; + ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", output.getName().c_str(), &output, me->getName().c_str(), me); + return (*me = output.get()).connected(); + } + + template bool connectTo(Derived& input) const { + const Derived *me = dynamic_cast(this); + if (!me) return false; + ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", me->getName().c_str(), me, input.getName().c_str(), &input); + return (input = me->get()).connected(); + } + +private: + QuadrotorInterface *interface_; + const std::string name_; + const std::string field_; + boost::shared_ptr my_; + +protected: + mutable bool new_value_; + bool wasNew() const { bool old = new_value_; new_value_ = false; return old; } +}; +typedef boost::shared_ptr CommandHandlePtr; + +namespace internal { + template struct FieldAccessor { + static typename Derived::ValueType *get(void *) { return 0; } + }; +} + +template +class CommandHandle_ : public Parent +{ +public: + typedef T ValueType; + typedef CommandHandle_ Base; + + CommandHandle_() : command_(0) {} + CommandHandle_(const Parent &other) : Parent(other), command_(0) {} + CommandHandle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Parent(interface, name, field), command_(0) {} + virtual ~CommandHandle_() {} + + virtual bool connected() const { return get(); } + virtual void reset() { command_ = 0; Parent::reset(); } + + using Parent::operator=; + Derived& operator=(ValueType *source) { command_ = source; return static_cast(*this); } + ValueType *get() const { return command_ ? command_ : internal::FieldAccessor::get(Parent::get()); } + ValueType &operator*() const { return *command_; } + + ValueType& command() { return *get(); } + const ValueType& getCommand() const { this->new_value_ = false; return *get(); } + void setCommand(const ValueType& command) { this->new_value_ = true; *get() = command; } + bool getCommand(ValueType& command) const { command = *get(); return this->wasNew(); } + + bool update(ValueType& command) const { + if (!connected()) return false; + command = getCommand(); + return true; + } + +protected: + ValueType *command_; +}; + +class PoseCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + PoseCommandHandle() {} + PoseCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {} + PoseCommandHandle(Pose* command) { *this = command; } + virtual ~PoseCommandHandle() {} +}; +typedef boost::shared_ptr PoseCommandHandlePtr; + +class HorizontalPositionCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + HorizontalPositionCommandHandle() {} + HorizontalPositionCommandHandle(const PoseCommandHandle& other) : Base(other) {} + HorizontalPositionCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.xy") {} + HorizontalPositionCommandHandle(Point* command) { *this = command; } + virtual ~HorizontalPositionCommandHandle() {} + + using Base::getCommand; + virtual bool getCommand(double &x, double &y) const { + x = get()->x; + y = get()->y; + return wasNew(); + } + + using Base::setCommand; + virtual void setCommand(double x, double y) + { + this->new_value_ = true; + get()->x = x; + get()->y = y; + } + + using Base::update; + bool update(Pose& command) const { + if (!connected()) return false; + getCommand(command.position.x, command.position.y); + return true; + } + + void getError(const PoseHandle &pose, double &x, double &y) const; +}; +typedef boost::shared_ptr HorizontalPositionCommandHandlePtr; + +namespace internal { + template <> struct FieldAccessor { + static Point *get(Pose *pose) { return &(pose->position); } + }; +} + +class HeightCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + HeightCommandHandle() {} + HeightCommandHandle(const PoseCommandHandle& other) : Base(other) {} + HeightCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.z") {} + HeightCommandHandle(double *command) { *this = command; } + virtual ~HeightCommandHandle() {} + + using Base::update; + bool update(Pose& command) const { + if (!connected()) return false; + command.position.z = getCommand(); + return true; + } + + double getError(const PoseHandle &pose) const; +}; +typedef boost::shared_ptr HeightCommandHandlePtr; + +namespace internal { + template <> struct FieldAccessor { + static double *get(Pose *pose) { return &(pose->position.z); } + }; +} + +class HeadingCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + HeadingCommandHandle() {} + HeadingCommandHandle(const PoseCommandHandle& other) : Base(other), scalar_(0) {} + HeadingCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "orientation.yaw"), scalar_(0) {} + HeadingCommandHandle(Quaternion *command) { *this = command; } + virtual ~HeadingCommandHandle() {} + + using Base::getCommand; + double getCommand() const; + + using Base::setCommand; + void setCommand(double command); + + using Base::update; + bool update(Pose& command) const; + + double getError(const PoseHandle &pose) const; + +protected: + double *scalar_; +}; +typedef boost::shared_ptr HeadingCommandHandlePtr; + +namespace internal { + template <> struct FieldAccessor { + static Quaternion *get(Pose *pose) { return &(pose->orientation); } + }; +} + +class TwistCommandHandle : public CommandHandle_ +{ +public: + + using Base::operator=; + + TwistCommandHandle() {} + TwistCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {} + TwistCommandHandle(Twist* command) { *this = command; } + virtual ~TwistCommandHandle() {} +}; +typedef boost::shared_ptr TwistCommandHandlePtr; + +class HorizontalVelocityCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + HorizontalVelocityCommandHandle() {} + HorizontalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {} + HorizontalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.xy") {} + HorizontalVelocityCommandHandle(Vector3* command) { *this = command; } + virtual ~HorizontalVelocityCommandHandle() {} + + using Base::getCommand; + bool getCommand(double &x, double &y) const { + x = get()->x; + y = get()->y; + return true; + } + + using Base::setCommand; + void setCommand(double x, double y) + { + get()->x = x; + get()->y = y; + } + + using Base::update; + bool update(Twist& command) const { + if (!connected()) return false; + getCommand(command.linear.x, command.linear.y); + return true; + } +}; +typedef boost::shared_ptr HorizontalVelocityCommandHandlePtr; + +namespace internal { + template <> struct FieldAccessor { + static Vector3 *get(Twist *twist) { return &(twist->linear); } + }; +} + +class VerticalVelocityCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + VerticalVelocityCommandHandle() {} + VerticalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {} + VerticalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.z") {} + VerticalVelocityCommandHandle(double* command) { *this = command; } + virtual ~VerticalVelocityCommandHandle() {} + + using Base::update; + bool update(Twist& command) const { + if (!connected()) return false; + command.linear.z = *get(); + return true; + } +}; +typedef boost::shared_ptr VerticalVelocityCommandHandlePtr; + +namespace internal { + template <> struct FieldAccessor { + static double *get(Twist *twist) { return &(twist->linear.z); } + }; +} + +class AngularVelocityCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + AngularVelocityCommandHandle() {} + AngularVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {} + AngularVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "angular.z") {} + AngularVelocityCommandHandle(double* command) { *this = command; } + virtual ~AngularVelocityCommandHandle() {} + + using Base::update; + bool update(Twist& command) const { + if (!connected()) return false; + command.linear.z = *get(); + return true; + } +}; +typedef boost::shared_ptr AngularVelocityCommandHandlePtr; + +namespace internal { + template <> struct FieldAccessor { + static double *get(Twist *twist) { return &(twist->angular.z); } + }; +} + +class WrenchCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + WrenchCommandHandle() {} + WrenchCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {} + virtual ~WrenchCommandHandle() {} +}; +typedef boost::shared_ptr WrenchCommandHandlePtr; + +class MotorCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + MotorCommandHandle() {} + MotorCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {} + virtual ~MotorCommandHandle() {} +}; +typedef boost::shared_ptr MotorCommandHandlePtr; + +class AttitudeCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + AttitudeCommandHandle() {} + AttitudeCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {} + virtual ~AttitudeCommandHandle() {} +}; +typedef boost::shared_ptr AttitudeCommandHandlePtr; + +class YawrateCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + YawrateCommandHandle() {} + YawrateCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {} + virtual ~YawrateCommandHandle() {} +}; +typedef boost::shared_ptr YawrateCommandHandlePtr; + +class ThrustCommandHandle : public CommandHandle_ +{ +public: + using Base::operator=; + + ThrustCommandHandle() {} + ThrustCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {} + virtual ~ThrustCommandHandle() {} +}; +typedef boost::shared_ptr ThrustCommandHandlePtr; + +} // namespace hector_quadrotor_controller + +#endif // HECTOR_QUADROTOR_CONTROLLER_HANDLES_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h new file mode 100644 index 0000000..f7601c3 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h @@ -0,0 +1,45 @@ +#ifndef HECTOR_QUADROTOR_CONTROLLER_PID_H +#define HECTOR_QUADROTOR_CONTROLLER_PID_H + +#include + +namespace hector_quadrotor_controller { + +class PID +{ +public: + struct parameters { + parameters(); + bool enabled; + double time_constant; + double k_p; + double k_i; + double k_d; + double limit_i; + double limit_output; + } parameters_; + + struct state { + state(); + double p, i, d; + double input, dinput; + double dx; + } state_; + +public: + PID(); + PID(const parameters& parameters); + ~PID(); + + void init(const ros::NodeHandle ¶m_nh); + void reset(); + + double update(double input, double x, double dx, const ros::Duration& dt); + double update(double error, double dx, const ros::Duration& dt); + + double getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt); +}; + +} // namespace hector_quadrotor_controller + +#endif // HECTOR_QUADROTOR_CONTROLLER_PID_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/quadrotor_interface.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/quadrotor_interface.h new file mode 100644 index 0000000..e077e66 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/quadrotor_interface.h @@ -0,0 +1,143 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H +#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H + +#include + +#include + +#include +#include + +namespace hector_quadrotor_controller { + +using namespace hardware_interface; + +class QuadrotorInterface : public HardwareInterface +{ +public: + QuadrotorInterface(); + virtual ~QuadrotorInterface(); + + virtual PoseHandlePtr getPose() { return PoseHandlePtr(); } + virtual TwistHandlePtr getTwist() { return TwistHandlePtr(); } + virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(); } + virtual ImuHandlePtr getSensorImu() { return ImuHandlePtr(); } + virtual MotorStatusHandlePtr getMotorStatus() { return MotorStatusHandlePtr(); } + + virtual bool getMassAndInertia(double &mass, double inertia[3]) { return false; } + + template boost::shared_ptr getHandle() + { + return boost::shared_ptr(new HandleType(this)); + } + + template boost::shared_ptr addInput(const std::string& name) + { + boost::shared_ptr input = getInput(name); + if (input) return input; + + // create new input handle + input.reset(new HandleType(this, name)); + inputs_[name] = input; + + // connect to output of same name + if (outputs_.count(name)) { + boost::shared_ptr output = boost::dynamic_pointer_cast(outputs_.at(name)); + output->connectTo(*input); + } + + return input; + } + + template boost::shared_ptr addOutput(const std::string& name) + { + boost::shared_ptr output = getOutput(name); + if (output) return output; + + // create new output handle + output.reset(new HandleType(this, name)); + outputs_[name] = output; + *output = output->ownData(new typename HandleType::ValueType()); + + //claim resource + claim(name); + + // connect to output of same name + if (inputs_.count(name)) { + boost::shared_ptr input = boost::dynamic_pointer_cast(inputs_.at(name)); + output->connectTo(*input); + } + + return output; + } + + template boost::shared_ptr getOutput(const std::string& name) const + { + if (!outputs_.count(name)) return boost::shared_ptr(); + return boost::static_pointer_cast(outputs_.at(name)); + } + + template boost::shared_ptr getInput(const std::string& name) const + { + if (!inputs_.count(name)) return boost::shared_ptr(); + return boost::static_pointer_cast(inputs_.at(name)); + } + + template typename HandleType::ValueType const* getCommand(const std::string& name) const + { + boost::shared_ptr output = getOutput(name); + if (!output || !output->connected()) return 0; + return &(output->command()); + } + + virtual const Pose *getPoseCommand() const; + virtual const Twist *getTwistCommand() const; + virtual const Wrench *getWrenchCommand() const; + virtual const MotorCommand *getMotorCommand() const; + + bool enabled(const CommandHandle *handle) const; + bool start(const CommandHandle *handle); + void stop(const CommandHandle *handle); + + void disconnect(const CommandHandle *handle); + +private: +// typedef std::list< CommandHandlePtr > HandleList; +// std::map inputs_; +// std::map outputs_; + std::map inputs_; + std::map outputs_; + std::map enabled_; +}; + +} // namespace hector_quadrotor_controller + +#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/launch/controller.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/launch/controller.launch new file mode 100644 index 0000000..87da66d --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/launch/controller.launch @@ -0,0 +1,7 @@ + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/package.xml new file mode 100644 index 0000000..6c43842 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/package.xml @@ -0,0 +1,42 @@ + + hector_quadrotor_controller + 0.3.5 + hector_quadrotor_controller provides libraries and a node for quadrotor control using ros_control. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_controller + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + + + catkin + + + roscpp + geometry_msgs + sensor_msgs + nav_msgs + hector_uav_msgs + std_srvs + hardware_interface + controller_interface + + + roscpp + geometry_msgs + sensor_msgs + nav_msgs + hector_uav_msgs + std_srvs + hardware_interface + controller_interface + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/params/controller.yaml b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/params/controller.yaml new file mode 100644 index 0000000..981e152 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/params/controller.yaml @@ -0,0 +1,50 @@ +controller: + pose: + type: hector_quadrotor_controller/PoseController + xy: + k_p: 2.0 + k_i: 0.0 + k_d: 0.0 + limit_output: 5.0 + z: + k_p: 2.0 + k_i: 0.0 + k_d: 0.0 + limit_output: 5.0 + yaw: + k_p: 2.0 + k_i: 0.0 + k_d: 0.0 + limit_output: 1.0 + twist: + type: hector_quadrotor_controller/TwistController + linear/xy: + k_p: 5.0 + k_i: 1.0 + k_d: 0.0 + limit_output: 10.0 + time_constant: 0.05 + linear/z: + k_p: 5.0 + k_i: 1.0 + k_d: 0.0 + limit_output: 10.0 + time_constant: 0.05 + angular/xy: + k_p: 10.0 + k_i: 5.0 + k_d: 5.0 + time_constant: 0.01 + angular/z: + k_p: 5.0 + k_i: 2.5 + k_d: 0.0 + limit_output: 3.0 + time_constant: 0.1 + limits: + load_factor: 1.5 + force/z: 30.0 + torque/xy: 10.0 + torque/z: 1.0 + motor: + type: hector_quadrotor_controller/MotorController diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/plugin.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/plugin.xml new file mode 100644 index 0000000..954de2b --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/plugin.xml @@ -0,0 +1,23 @@ + + + + + The PoseController controls the quadrotor's pose and outputs velocity commands. + + + + + + + The TwistController controls the quadrotor's twist (linear and angular velocity) by applying torque and thrust similar to a real quadrotor. + + + + + + + The MotorController calculates the output voltages of the four motors from the commanded thrust and torque using a simple linearized model. + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/motor_controller.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/motor_controller.cpp new file mode 100644 index 0000000..5f9e466 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/motor_controller.cpp @@ -0,0 +1,194 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include +#include + +#include +#include + +namespace hector_quadrotor_controller { + +using namespace controller_interface; + +class MotorController : public controller_interface::Controller +{ +public: + MotorController() + : node_handle_(0) + {} + + ~MotorController() + { + if (node_handle_) { + node_handle_->shutdown(); + delete node_handle_; + node_handle_ = 0; + } + } + + bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) + { + // get interface handles + wrench_input_ = interface->addInput("wrench"); + motor_output_ = interface->addOutput("motor"); + + // initialize NodeHandle + delete node_handle_; + node_handle_ = new ros::NodeHandle(root_nh); + + // load parameters + controller_nh.getParam("force_per_voltage", parameters_.force_per_voltage = 0.559966216); + controller_nh.getParam("torque_per_voltage", parameters_.torque_per_voltage = 7.98598e-3); + controller_nh.getParam("lever", parameters_.lever = 0.275); + root_nh.param("base_link_frame", base_link_frame_, "base_link"); + + // TODO: calculate these parameters from the quadrotor_propulsion parameters +// quadrotor_propulsion: +// k_t: 0.015336864714397 +// k_m: -7.011631909766668e-5 + +// CT2s: -1.3077e-2 +// CT1s: -2.5224e-4 +// CT0s: 1.538190483976698e-5 + + return true; + } + + void reset() + { + wrench_.wrench.force.x = 0.0; + wrench_.wrench.force.y = 0.0; + wrench_.wrench.force.z = 0.0; + wrench_.wrench.torque.x = 0.0; + wrench_.wrench.torque.y = 0.0; + wrench_.wrench.torque.z = 0.0; + + motor_.force.assign(4, 0.0); + motor_.torque.assign(4, 0.0); + motor_.frequency.clear(); + motor_.voltage.assign(4, 0.0); + } + + void wrenchCommandCallback(const geometry_msgs::WrenchStampedConstPtr& command) + { + wrench_ = *command; + if (wrench_.header.stamp.isZero()) wrench_.header.stamp = ros::Time::now(); + + // start controller if it not running + if (!isRunning()) this->startRequest(wrench_.header.stamp); + } + + void starting(const ros::Time &time) + { + reset(); + motor_output_->start(); + } + + void stopping(const ros::Time &time) + { + motor_output_->stop(); + } + + void update(const ros::Time& time, const ros::Duration& period) + { + // Get wrench command input + if (wrench_input_->connected() && wrench_input_->enabled()) { + wrench_.wrench = wrench_input_->getCommand(); + } + + // std::cout << "motor_controller:update" << std::endl; //added by zhangshuai@2018.12.18 + // std::cout<<"wrench_.wrench.force.x:"< 0.0) { + + double nominal_thrust_per_motor = wrench_.wrench.force.z / 4.0; + motor_.force[0] = nominal_thrust_per_motor - wrench_.wrench.torque.y / 2.0 / parameters_.lever; + motor_.force[1] = nominal_thrust_per_motor - wrench_.wrench.torque.x / 2.0 / parameters_.lever; + motor_.force[2] = nominal_thrust_per_motor + wrench_.wrench.torque.y / 2.0 / parameters_.lever; + motor_.force[3] = nominal_thrust_per_motor + wrench_.wrench.torque.x / 2.0 / parameters_.lever; + + double nominal_torque_per_motor = wrench_.wrench.torque.z / 4.0; + motor_.voltage[0] = motor_.force[0] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage; + motor_.voltage[1] = motor_.force[1] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage; + motor_.voltage[2] = motor_.force[2] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage; + motor_.voltage[3] = motor_.force[3] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage; + + motor_.torque[0] = motor_.voltage[0] * parameters_.torque_per_voltage; + motor_.torque[1] = motor_.voltage[1] * parameters_.torque_per_voltage; + motor_.torque[2] = motor_.voltage[2] * parameters_.torque_per_voltage; + motor_.torque[3] = motor_.voltage[3] * parameters_.torque_per_voltage; + + if (motor_.voltage[0] < 0.0) motor_.voltage[0] = 0.0; + if (motor_.voltage[1] < 0.0) motor_.voltage[1] = 0.0; + if (motor_.voltage[2] < 0.0) motor_.voltage[2] = 0.0; + if (motor_.voltage[3] < 0.0) motor_.voltage[3] = 0.0; + + } else { + reset(); + } + + // set wrench output + motor_.header.stamp = time; + motor_.header.frame_id = "base_link"; + motor_output_->setCommand(motor_); + } + +private: + WrenchCommandHandlePtr wrench_input_; + MotorCommandHandlePtr motor_output_; + + ros::NodeHandle *node_handle_; + ros::Subscriber wrench_subscriber_; + ros::ServiceServer engage_service_server_; + ros::ServiceServer shutdown_service_server_; + + geometry_msgs::WrenchStamped wrench_; + hector_uav_msgs::MotorCommand motor_; + std::string base_link_frame_; + + struct { + double force_per_voltage; // coefficient for linearized volts to force conversion for a single motor [N / V] + double torque_per_voltage; // coefficient for linearized volts to force conversion for a single motor [Nm / V] + double lever; // the lever arm from origin to the motor axes (symmetry assumption) [m] + } parameters_; +}; + +} // namespace hector_quadrotor_controller + +#include +PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::MotorController, controller_interface::ControllerBase) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pid.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pid.cpp new file mode 100644 index 0000000..bc8e3e5 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pid.cpp @@ -0,0 +1,152 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +namespace hector_quadrotor_controller { + +PID::state::state() + : p(std::numeric_limits::quiet_NaN()) + , i(0.0) + , d(std::numeric_limits::quiet_NaN()) + , input(std::numeric_limits::quiet_NaN()) + , dinput(0.0) + , dx(std::numeric_limits::quiet_NaN()) +{ +} + +PID::parameters::parameters() + : enabled(true) + , time_constant(0.0) + , k_p(0.0) + , k_i(0.0) + , k_d(0.0) + , limit_i(std::numeric_limits::quiet_NaN()) + , limit_output(std::numeric_limits::quiet_NaN()) +{ +} + +PID::PID() +{} + +PID::PID(const parameters& params) + : parameters_(params) +{} + +PID::~PID() +{} + +void PID::init(const ros::NodeHandle ¶m_nh) +{ + param_nh.getParam("enabled", parameters_.enabled); + param_nh.getParam("k_p", parameters_.k_p); + param_nh.getParam("k_i", parameters_.k_i); + param_nh.getParam("k_d", parameters_.k_d); + param_nh.getParam("limit_i", parameters_.limit_i); + param_nh.getParam("limit_output", parameters_.limit_output); + param_nh.getParam("time_constant", parameters_.time_constant); +} + +void PID::reset() +{ + state_ = state(); +} + +template static inline T& checknan(T& value) +{ + if (std::isnan(value)) value = T(); + return value; +} + +double PID::update(double input, double x, double dx, const ros::Duration& dt) +{ + if (!parameters_.enabled) return 0.0; + double dt_sec = dt.toSec(); + + // low-pass filter input + if (std::isnan(state_.input)) state_.input = input; + if (dt_sec + parameters_.time_constant > 0.0) { + state_.dinput = (input - state_.input) / (dt_sec + parameters_.time_constant); + state_.input = (dt_sec * input + parameters_.time_constant * state_.input) / (dt_sec + parameters_.time_constant); + } + + return update(state_.input - x, dx, dt); +} + +double PID::update(double error, double dx, const ros::Duration& dt) +{ + if (!parameters_.enabled) return 0.0; + if (std::isnan(error)) return 0.0; + double dt_sec = dt.toSec(); + + // integral error + state_.i += error * dt_sec; + if (parameters_.limit_i > 0.0) + { + if (state_.i > parameters_.limit_i) state_.i = parameters_.limit_i; + if (state_.i < -parameters_.limit_i) state_.i = -parameters_.limit_i; + } + + // differential error + if (dt_sec > 0.0 && !std::isnan(state_.p) && !std::isnan(state_.dx)) { + state_.d = (error - state_.p) / dt_sec + state_.dx - dx; + } else { + state_.d = -dx; + } + state_.dx = dx; + + // proportional error + state_.p = error; + + // calculate output... + double output = parameters_.k_p * state_.p + parameters_.k_i * state_.i + parameters_.k_d * state_.d; + int antiwindup = 0; + if (parameters_.limit_output > 0.0) + { + if (output > parameters_.limit_output) { output = parameters_.limit_output; antiwindup = 1; } + if (output < -parameters_.limit_output) { output = -parameters_.limit_output; antiwindup = -1; } + } + if (antiwindup && (error * dt_sec * antiwindup > 0.0)) state_.i -= error * dt_sec; + + checknan(output); + return output; +} + +double PID::getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt) +{ + double dt_sec = dt.toSec(); + filtered_error = checknan(filtered_error); + if (dt_sec + time_constant > 0.0) { + filtered_error = (time_constant * filtered_error + dt_sec * state_.p) / (dt_sec + time_constant); + } + return filtered_error; +} + +} // namespace hector_quadrotor_controller + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pose_controller.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pose_controller.cpp new file mode 100644 index 0000000..072f730 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pose_controller.cpp @@ -0,0 +1,205 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include + +#include +#include + +#include +#include + +namespace hector_quadrotor_controller { + +using namespace controller_interface; + +class PoseController : public controller_interface::Controller +{ +public: + PoseController() {} + + ~PoseController() {} + + bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) + { + // get interface handles + pose_ = interface->getPose(); + twist_ = interface->getTwist(); + + pose_input_ = interface->addInput("pose"); + twist_input_ = interface->addInput("pose/twist"); + twist_limit_ = interface->addInput("pose/twist_limit"); + twist_output_ = interface->addOutput("twist"); + + node_handle_ = root_nh; + + // subscribe to commanded pose and velocity + pose_subscriber_ = node_handle_.subscribe("command/pose", 1, boost::bind(&PoseController::poseCommandCallback, this, _1)); + twist_subscriber_ = node_handle_.subscribe("command/twist", 1, boost::bind(&PoseController::twistCommandCallback, this, _1)); + + // initialize PID controllers + pid_.x.init(ros::NodeHandle(controller_nh, "xy")); + pid_.y.init(ros::NodeHandle(controller_nh, "xy")); + pid_.z.init(ros::NodeHandle(controller_nh, "z")); + pid_.yaw.init(ros::NodeHandle(controller_nh, "yaw")); + + return true; + } + + void reset() + { + pid_.x.reset(); + pid_.y.reset(); + pid_.z.reset(); + pid_.yaw.reset(); + } + + void poseCommandCallback(const geometry_msgs::PoseStampedConstPtr& command) + { + pose_command_ = *command; + if (!(pose_input_->connected())) *pose_input_ = &(pose_command_.pose); + pose_input_->start(); + + ros::Time start_time = command->header.stamp; + if (start_time.isZero()) start_time = ros::Time::now(); + if (!isRunning()) this->startRequest(start_time); + } + + void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command) + { + twist_command_ = *command; + if (!(twist_input_->connected())) *twist_input_ = &(twist_command_.twist); + twist_input_->start(); + + ros::Time start_time = command->header.stamp; + if (start_time.isZero()) start_time = ros::Time::now(); + if (!isRunning()) this->startRequest(start_time); + } + + void starting(const ros::Time &time) + { + reset(); + twist_output_->start(); + } + + void stopping(const ros::Time &time) + { + twist_output_->stop(); + } + + void update(const ros::Time& time, const ros::Duration& period) + { + // std::cout << "pose_controller:update" << std::endl; //added by zhangshuai@2018.12.18 + Twist output; + + // check command timeout + // TODO + + // return if no pose command is available + if (pose_input_->enabled()) { + // control horizontal position + double error_n, error_w; + HorizontalPositionCommandHandle(*pose_input_).getError(*pose_, error_n, error_w); + output.linear.x = pid_.x.update(error_n, twist_->twist().linear.x, period); + output.linear.y = pid_.y.update(error_w, twist_->twist().linear.y, period); + + // control height + output.linear.z = pid_.z.update(HeightCommandHandle(*pose_input_).getError(*pose_), twist_->twist().linear.z, period); + + // control yaw angle + output.angular.z = pid_.yaw.update(HeadingCommandHandle(*pose_input_).getError(*pose_), twist_->twist().angular.z, period); + } + + // add twist command if available + if (twist_input_->enabled()) + { + output.linear.x += twist_input_->getCommand().linear.x; + output.linear.y += twist_input_->getCommand().linear.y; + output.linear.z += twist_input_->getCommand().linear.z; + output.angular.x += twist_input_->getCommand().angular.x; + output.angular.y += twist_input_->getCommand().angular.y; + output.angular.z += twist_input_->getCommand().angular.z; + } + + // limit twist + if (twist_limit_->enabled()) + { + double linear_xy = sqrt(output.linear.x*output.linear.x + output.linear.y*output.linear.y); + double limit_linear_xy = std::max(twist_limit_->get()->linear.x, twist_limit_->get()->linear.y); + if (limit_linear_xy > 0.0 && linear_xy > limit_linear_xy) { + output.linear.x *= limit_linear_xy / linear_xy; + output.linear.y *= limit_linear_xy / linear_xy; + } + if (twist_limit_->get()->linear.z > 0.0 && fabs(output.linear.z) > twist_limit_->get()->linear.z) { + output.linear.z *= twist_limit_->get()->linear.z / fabs(output.linear.z); + } + double angular_xy = sqrt(output.angular.x*output.angular.x + output.angular.y*output.angular.y); + double limit_angular_xy = std::max(twist_limit_->get()->angular.x, twist_limit_->get()->angular.y); + if (limit_angular_xy > 0.0 && angular_xy > limit_angular_xy) { + output.angular.x *= limit_angular_xy / angular_xy; + output.angular.y *= limit_angular_xy / angular_xy; + } + if (twist_limit_->get()->angular.z > 0.0 && fabs(output.angular.z) > twist_limit_->get()->angular.z) { + output.angular.z *= twist_limit_->get()->angular.z / fabs(output.angular.z); + } + } + + // set twist output + twist_output_->setCommand(output); + } + +private: + PoseHandlePtr pose_; + PoseCommandHandlePtr pose_input_; + TwistHandlePtr twist_; + TwistCommandHandlePtr twist_input_; + TwistCommandHandlePtr twist_limit_; + TwistCommandHandlePtr twist_output_; + + geometry_msgs::PoseStamped pose_command_; + geometry_msgs::TwistStamped twist_command_; + + ros::NodeHandle node_handle_; + ros::Subscriber pose_subscriber_; + ros::Subscriber twist_subscriber_; + + struct { + PID x; + PID y; + PID z; + PID yaw; + } pid_; +}; + +} // namespace hector_quadrotor_controller + +#include +PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::PoseController, controller_interface::ControllerBase) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/quadrotor_interface.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/quadrotor_interface.cpp new file mode 100644 index 0000000..4cdef65 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/quadrotor_interface.cpp @@ -0,0 +1,195 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include + +#include + +namespace hector_quadrotor_controller { + +QuadrotorInterface::QuadrotorInterface() +{} + +QuadrotorInterface::~QuadrotorInterface() +{} + +bool QuadrotorInterface::enabled(const CommandHandle *handle) const +{ + if (!handle || !handle->connected()) return false; + std::string resource = handle->getName(); + return enabled_.count(resource) > 0; +} + +bool QuadrotorInterface::start(const CommandHandle *handle) +{ + if (!handle || !handle->connected()) return false; + if (enabled(handle)) return true; + std::string resource = handle->getName(); + enabled_[resource] = handle; + ROS_DEBUG_NAMED("quadrotor_interface", "Enabled %s control", resource.c_str()); + return true; +} + +void QuadrotorInterface::stop(const CommandHandle *handle) +{ + if (!handle) return; + if (!enabled(handle)) return; + std::string resource = handle->getName(); + std::map::iterator it = enabled_.lower_bound(resource); + if (it != enabled_.end() && it->second == handle) enabled_.erase(it); + ROS_DEBUG_NAMED("quadrotor_interface", "Disabled %s control", resource.c_str()); +} + +void QuadrotorInterface::disconnect(const CommandHandle *handle) +{ + if (!handle) return; + std::string resource = handle->getName(); + if (inputs_.count(resource)) { + const CommandHandlePtr& input = inputs_.at(resource); + if (input.get() != handle) input->reset(); + } + if (outputs_.count(resource)) { + const CommandHandlePtr& output = outputs_.at(resource); + if (output.get() != handle) output->reset(); + } +} + +const Pose *QuadrotorInterface::getPoseCommand() const { return getCommand("pose"); } +const Twist *QuadrotorInterface::getTwistCommand() const { return getCommand("twist"); } +const Wrench *QuadrotorInterface::getWrenchCommand() const { return getCommand("wrench"); } +const MotorCommand *QuadrotorInterface::getMotorCommand() const { return getCommand("motor"); } + +void PoseHandle::getEulerRPY(double &roll, double &pitch, double &yaw) const +{ + const Quaternion::_w_type& w = pose().orientation.w; + const Quaternion::_x_type& x = pose().orientation.x; + const Quaternion::_y_type& y = pose().orientation.y; + const Quaternion::_z_type& z = pose().orientation.z; + roll = atan2(2.*y*z + 2.*w*x, z*z - y*y - x*x + w*w); + pitch = -asin(2.*x*z - 2.*w*y); + yaw = atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y); +} + +double PoseHandle::getYaw() const +{ + const Quaternion::_w_type& w = pose().orientation.w; + const Quaternion::_x_type& x = pose().orientation.x; + const Quaternion::_y_type& y = pose().orientation.y; + const Quaternion::_z_type& z = pose().orientation.z; + return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y); +} + +Vector3 PoseHandle::toBody(const Vector3& nav) const +{ + const Quaternion::_w_type& w = pose().orientation.w; + const Quaternion::_x_type& x = pose().orientation.x; + const Quaternion::_y_type& y = pose().orientation.y; + const Quaternion::_z_type& z = pose().orientation.z; + Vector3 body; + body.x = (w*w+x*x-y*y-z*z) * nav.x + (2.*x*y + 2.*w*z) * nav.y + (2.*x*z - 2.*w*y) * nav.z; + body.y = (2.*x*y - 2.*w*z) * nav.x + (w*w-x*x+y*y-z*z) * nav.y + (2.*y*z + 2.*w*x) * nav.z; + body.z = (2.*x*z + 2.*w*y) * nav.x + (2.*y*z - 2.*w*x) * nav.y + (w*w-x*x-y*y+z*z) * nav.z; + return body; +} + +Vector3 PoseHandle::fromBody(const Vector3& body) const +{ + const Quaternion::_w_type& w = pose().orientation.w; + const Quaternion::_x_type& x = pose().orientation.x; + const Quaternion::_y_type& y = pose().orientation.y; + const Quaternion::_z_type& z = pose().orientation.z; + Vector3 nav; + nav.x = (w*w+x*x-y*y-z*z) * body.x + (2.*x*y - 2.*w*z) * body.y + (2.*x*z + 2.*w*y) * body.z; + nav.y = (2.*x*y + 2.*w*z) * body.x + (w*w-x*x+y*y-z*z) * body.y + (2.*y*z - 2.*w*x) * body.z; + nav.z = (2.*x*z - 2.*w*y) * body.x + (2.*y*z + 2.*w*x) * body.y + (w*w-x*x-y*y+z*z) * body.z; + return nav; +} + +void HorizontalPositionCommandHandle::getError(const PoseHandle &pose, double &x, double &y) const +{ + getCommand(x, y); + x -= pose.get()->position.x; + y -= pose.get()->position.y; +} + +double HeightCommandHandle::getError(const PoseHandle &pose) const +{ + return getCommand() - pose.get()->position.z; +} + +void HeadingCommandHandle::setCommand(double command) +{ + if (get()) { + get()->x = 0.0; + get()->y = 0.0; + get()->z = sin(command / 2.); + get()->w = cos(command / 2.); + } + + if (scalar_) { + *scalar_ = command; + } +} + +double HeadingCommandHandle::getCommand() const { + if (scalar_) return *scalar_; + const Quaternion::_w_type& w = get()->w; + const Quaternion::_x_type& x = get()->x; + const Quaternion::_y_type& y = get()->y; + const Quaternion::_z_type& z = get()->z; + return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y); +} + +bool HeadingCommandHandle::update(Pose& command) const { + if (get()) { + command.orientation = *get(); + return true; + } + if (scalar_) { + command.orientation.x = 0.0; + command.orientation.y = 0.0; + command.orientation.z = sin(*scalar_ / 2.); + command.orientation.x = cos(*scalar_ / 2.); + return true; + } + return false; +} + +double HeadingCommandHandle::getError(const PoseHandle &pose) const { + static const double M_2PI = 2.0 * M_PI; + double error = getCommand() - pose.getYaw() + M_PI; + error -= floor(error / M_2PI) * M_2PI; + return error - M_PI; +} + +bool CommandHandle::enabled() { return interface_->enabled(this); } +bool CommandHandle::start() { return interface_->start(this); } +void CommandHandle::stop() { interface_->stop(this); } +void CommandHandle::disconnect() { interface_->disconnect(this); } + +} // namespace hector_quadrotor_controller diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller (复件).cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller (复件).cpp new file mode 100644 index 0000000..44107e9 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller (复件).cpp @@ -0,0 +1,328 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include + +#include + +namespace hector_quadrotor_controller { + +using namespace controller_interface; + +class TwistController : public controller_interface::Controller +{ +public: + TwistController() + {} + + ~TwistController() + {} + + bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) + { + // get interface handles + pose_ = interface->getPose(); + twist_ = interface->getTwist(); + acceleration_ = interface->getAcceleration(); + twist_input_ = interface->addInput("twist"); + wrench_output_ = interface->addOutput("wrench"); + node_handle_ = root_nh; + + // subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist) + twist_subscriber_ = node_handle_.subscribe("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1)); + cmd_vel_subscriber_ = node_handle_.subscribe("/fixedwing/cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1)); + + // engage/shutdown service servers + engage_service_server_ = node_handle_.advertiseService("engage", boost::bind(&TwistController::engageCallback, this, _1, _2)); + shutdown_service_server_ = node_handle_.advertiseService("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2)); + + // initialize PID controllers + pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy")); + pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy")); + pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z")); + pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy")); + pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy")); + pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z")); + + // load other parameters + controller_nh.getParam("auto_engage", auto_engage_ = true); + controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5); + controller_nh.getParam("limits/force/z", limits_.force.z); + controller_nh.getParam("limits/torque/xy", limits_.torque.x); + controller_nh.getParam("limits/torque/xy", limits_.torque.y); + controller_nh.getParam("limits/torque/z", limits_.torque.z); + root_nh.param("base_link_frame", base_link_frame_, "base_link"); + + // get mass and inertia from QuadrotorInterface + interface->getMassAndInertia(mass_, inertia_); + + command_given_in_stabilized_frame_ = false; + + return true; + } + + void reset() + { + pid_.linear.x.reset(); + pid_.linear.y.reset(); + pid_.linear.z.reset(); + pid_.angular.x.reset(); + pid_.angular.y.reset(); + pid_.angular.z.reset(); + + wrench_.wrench.force.x = 0.0; + wrench_.wrench.force.y = 0.0; + wrench_.wrench.force.z = 0.0; + wrench_.wrench.torque.x = 0.0; + wrench_.wrench.torque.y = 0.0; + wrench_.wrench.torque.z = 0.0; + + linear_z_control_error_ = 0.0; + motors_running_ = false; + } + + void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command) + { + boost::mutex::scoped_lock lock(command_mutex_); + + command_ = *command; + if (command_.header.stamp.isZero()) command_.header.stamp = ros::Time::now(); + command_given_in_stabilized_frame_ = false; + + // start controller if it not running + if (!isRunning()) this->startRequest(command_.header.stamp); + } + + void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr& command) + { + boost::mutex::scoped_lock lock(command_mutex_); + + command_.twist = *command; + command_.header.stamp = ros::Time::now(); + command_given_in_stabilized_frame_ = true; + + // start controller if it not running + if (!isRunning()) this->startRequest(command_.header.stamp); + } + + bool engageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) + { + boost::mutex::scoped_lock lock(command_mutex_); + + ROS_INFO_NAMED("twist_controller", "Engaging motors!"); + motors_running_ = true; + return true; + } + + bool shutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) + { + boost::mutex::scoped_lock lock(command_mutex_); + + ROS_INFO_NAMED("twist_controller", "Shutting down motors!"); + motors_running_ = false; + return true; + } + + void starting(const ros::Time &time) + { + reset(); + wrench_output_->start(); + } + + void stopping(const ros::Time &time) + { + wrench_output_->stop(); + } + + void update(const ros::Time& time, const ros::Duration& period) + { + boost::mutex::scoped_lock lock(command_mutex_); + + // Get twist command input + if (twist_input_->connected() && twist_input_->enabled()) { + command_.twist = twist_input_->getCommand(); + command_given_in_stabilized_frame_ = false; + } + + // Get current state and command + Twist command = command_.twist; + Twist twist = twist_->twist(); + Twist twist_body; + twist_body.linear = pose_->toBody(twist.linear); + twist_body.angular = pose_->toBody(twist.angular); + + // Transform to world coordinates if necessary (yaw only) + if (command_given_in_stabilized_frame_) { + double yaw = pose_->getYaw(); + Twist transformed = command; + transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y; + transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y; + transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y; + transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y; + command = transformed; + } + + // Get gravity and load factor + const double gravity = 9.8065; + double load_factor = 1. / ( pose_->pose().orientation.w * pose_->pose().orientation.w + - pose_->pose().orientation.x * pose_->pose().orientation.x + - pose_->pose().orientation.y * pose_->pose().orientation.y + + pose_->pose().orientation.z * pose_->pose().orientation.z ); + // Note: load_factor could be NaN or Inf...? + if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit)) load_factor = load_factor_limit; + + // Auto engage/shutdown + if (auto_engage_) { + if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0) { + motors_running_ = true; + ROS_INFO_NAMED("twist_controller", "Engaging motors!"); + } else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */) { + double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5); + if (linear_z_control_error_ > 0.0) linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown + if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit) { + motors_running_ = false; + ROS_INFO_NAMED("twist_controller", "Shutting down motors!"); + } else { + ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit); + } + } else { + linear_z_control_error_ = 0.0; + } + + // flip over? + if (motors_running_ && load_factor < 0.0) { + motors_running_ = false; + ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!"); + } + } + + // Update output + if (motors_running_) { + Vector3 acceleration_command; + acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period); + acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period); + acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity; + Vector3 acceleration_command_body = pose_->toBody(acceleration_command); + + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear: [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular: [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear: [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular: [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration: [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body: [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]"); + + wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period); + wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update( acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period); + wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update( command.angular.z, twist.angular.z, 0.0, period); + wrench_.wrench.force.x = 0.0; + wrench_.wrench.force.y = 0.0; + wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity); + + if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z) wrench_.wrench.force.z = limits_.force.z; + if (wrench_.wrench.force.z <= std::numeric_limits::min()) wrench_.wrench.force.z = std::numeric_limits::min(); + if (limits_.torque.x > 0.0) { + if (wrench_.wrench.torque.x > limits_.torque.x) wrench_.wrench.torque.x = limits_.torque.x; + if (wrench_.wrench.torque.x < -limits_.torque.x) wrench_.wrench.torque.x = -limits_.torque.x; + } + if (limits_.torque.y > 0.0) { + if (wrench_.wrench.torque.y > limits_.torque.y) wrench_.wrench.torque.y = limits_.torque.y; + if (wrench_.wrench.torque.y < -limits_.torque.y) wrench_.wrench.torque.y = -limits_.torque.y; + } + if (limits_.torque.z > 0.0) { + if (wrench_.wrench.torque.z > limits_.torque.z) wrench_.wrench.torque.z = limits_.torque.z; + if (wrench_.wrench.torque.z < -limits_.torque.z) wrench_.wrench.torque.z = -limits_.torque.z; + } + + ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force: [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque: [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]"); + + } else { + reset(); + } + + // set wrench output + wrench_.header.stamp = time; + wrench_.header.frame_id = base_link_frame_; + wrench_output_->setCommand(wrench_.wrench); + } + +private: + PoseHandlePtr pose_; + TwistHandlePtr twist_; + AccelerationHandlePtr acceleration_; + TwistCommandHandlePtr twist_input_; + WrenchCommandHandlePtr wrench_output_; + + ros::NodeHandle node_handle_; + ros::Subscriber twist_subscriber_; + ros::Subscriber cmd_vel_subscriber_; + ros::ServiceServer engage_service_server_; + ros::ServiceServer shutdown_service_server_; + + geometry_msgs::TwistStamped command_; + geometry_msgs::WrenchStamped wrench_; + bool command_given_in_stabilized_frame_; + std::string base_link_frame_; + + struct { + struct { + PID x; + PID y; + PID z; + } linear, angular; + } pid_; + + geometry_msgs::Wrench limits_; + bool auto_engage_; + double load_factor_limit; + double mass_; + double inertia_[3]; + + bool motors_running_; + double linear_z_control_error_; + boost::mutex command_mutex_; + +}; + +} // namespace hector_quadrotor_controller + +#include +PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller.cpp new file mode 100644 index 0000000..bc89237 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller.cpp @@ -0,0 +1,408 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include + +#include + +// #ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.19 +// #define _COUNT_TIME_HH_ + +// #ifndef _WIN32 +// #include +// #endif + +// #ifndef USE_COUNT_TIME +// #define USE_COUNT_TIME +// #endif + +// #endif + +namespace hector_quadrotor_controller +{ + +using namespace controller_interface; + +class TwistController : public controller_interface::Controller +{ +public: + TwistController() + { + } + + ~TwistController() + { + } + + bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) + { + // get interface handles + pose_ = interface->getPose(); + twist_ = interface->getTwist(); + acceleration_ = interface->getAcceleration(); + twist_input_ = interface->addInput("twist"); + wrench_output_ = interface->addOutput("wrench"); + node_handle_ = root_nh; + + // subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist) + twist_subscriber_ = node_handle_.subscribe("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1)); + cmd_vel_subscriber_ = node_handle_.subscribe("cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1)); + + // engage/shutdown service servers + engage_service_server_ = node_handle_.advertiseService("engage", boost::bind(&TwistController::engageCallback, this, _1, _2)); + shutdown_service_server_ = node_handle_.advertiseService("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2)); + + // initialize PID controllers + pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy")); + pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy")); + pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z")); + pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy")); + pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy")); + pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z")); + + // load other parameters + controller_nh.getParam("auto_engage", auto_engage_ = true); + controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5); + controller_nh.getParam("limits/force/z", limits_.force.z); + controller_nh.getParam("limits/torque/xy", limits_.torque.x); + controller_nh.getParam("limits/torque/xy", limits_.torque.y); + controller_nh.getParam("limits/torque/z", limits_.torque.z); + root_nh.param("base_link_frame", base_link_frame_, "base_link"); + + // get mass and inertia from QuadrotorInterface + interface->getMassAndInertia(mass_, inertia_); + + command_given_in_stabilized_frame_ = false; + + return true; + } + + void reset() + { + pid_.linear.x.reset(); + pid_.linear.y.reset(); + pid_.linear.z.reset(); + pid_.angular.x.reset(); + pid_.angular.y.reset(); + pid_.angular.z.reset(); + + wrench_.wrench.force.x = 0.0; + wrench_.wrench.force.y = 0.0; + wrench_.wrench.force.z = 0.0; + wrench_.wrench.torque.x = 0.0; + wrench_.wrench.torque.y = 0.0; + wrench_.wrench.torque.z = 0.0; + + linear_z_control_error_ = 0.0; + motors_running_ = false; + } + + void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr &command) + { + boost::mutex::scoped_lock lock(command_mutex_); + + command_ = *command; + if (command_.header.stamp.isZero()) + command_.header.stamp = ros::Time::now(); + command_given_in_stabilized_frame_ = false; + + // start controller if it not running + if (!isRunning()) + this->startRequest(command_.header.stamp); + } + + void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr &command) + { + boost::mutex::scoped_lock lock(command_mutex_); + + command_.twist = *command; + command_.header.stamp = ros::Time::now(); + command_given_in_stabilized_frame_ = true; + + // start controller if it not running + if (!isRunning()) + this->startRequest(command_.header.stamp); + } + + bool engageCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &) + { + boost::mutex::scoped_lock lock(command_mutex_); + + ROS_INFO_NAMED("twist_controller", "Engaging motors!"); + motors_running_ = true; + return true; + } + + bool shutdownCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &) + { + boost::mutex::scoped_lock lock(command_mutex_); + + ROS_INFO_NAMED("twist_controller", "Shutting down motors!"); + motors_running_ = false; + return true; + } + + void starting(const ros::Time &time) + { + reset(); + wrench_output_->start(); + } + + void stopping(const ros::Time &time) + { + wrench_output_->stop(); + } + +// int i=0; //added by zhangshuai@2018.12.18 +#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 + double twist_controller_update_time = 0.0; + int num = 0; +// double UpdateTime = 0; +// double ProcessTime = 0; +// double publishModelPoses_time = 0; +#endif + + void update(const ros::Time &time, const ros::Duration &period) + { +#ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19 + struct timeval tv; + double start_time, cur_time, tmp_time; + gettimeofday(&tv, NULL); + start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +#endif + + // std::cout<<"update:"<connected() && twist_input_->enabled()) + { + command_.twist = twist_input_->getCommand(); + command_given_in_stabilized_frame_ = false; + } + + // Get current state and command + Twist command = command_.twist; + Twist twist = twist_->twist(); + Twist twist_body; + twist_body.linear = pose_->toBody(twist.linear); + twist_body.angular = pose_->toBody(twist.angular); + + // Transform to world coordinates if necessary (yaw only) + if (command_given_in_stabilized_frame_) + { + double yaw = pose_->getYaw(); + Twist transformed = command; + transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y; + transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y; + transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y; + transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y; + command = transformed; + } + + // Get gravity and load factor + const double gravity = 9.8065; + double load_factor = 1. / (pose_->pose().orientation.w * pose_->pose().orientation.w - pose_->pose().orientation.x * pose_->pose().orientation.x - pose_->pose().orientation.y * pose_->pose().orientation.y + pose_->pose().orientation.z * pose_->pose().orientation.z); + // Note: load_factor could be NaN or Inf...? + if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit)) + load_factor = load_factor_limit; + + // Auto engage/shutdown + if (auto_engage_) + { + if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0) + { + motors_running_ = true; + ROS_INFO_NAMED("twist_controller", "Engaging motors!"); + } + else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */) + { + double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5); + if (linear_z_control_error_ > 0.0) + linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown + if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit) + { + motors_running_ = false; + ROS_INFO_NAMED("twist_controller", "Shutting down motors!"); + } + else + { + ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit); + } + } + else + { + linear_z_control_error_ = 0.0; + } + + // flip over? + if (motors_running_ && load_factor < 0.0) + { + motors_running_ = false; + ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!"); + } + } + + // Update output + if (motors_running_) + { + Vector3 acceleration_command; + acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period); + acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period); + acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity; + Vector3 acceleration_command_body = pose_->toBody(acceleration_command); + + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear: [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular: [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear: [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular: [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration: [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body: [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]"); + + wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period); + wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update(acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period); + wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update(command.angular.z, twist.angular.z, 0.0, period); + wrench_.wrench.force.x = 0.0; + wrench_.wrench.force.y = 0.0; + wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity); + + if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z) + wrench_.wrench.force.z = limits_.force.z; + if (wrench_.wrench.force.z <= std::numeric_limits::min()) + wrench_.wrench.force.z = std::numeric_limits::min(); + if (limits_.torque.x > 0.0) + { + if (wrench_.wrench.torque.x > limits_.torque.x) + wrench_.wrench.torque.x = limits_.torque.x; + if (wrench_.wrench.torque.x < -limits_.torque.x) + wrench_.wrench.torque.x = -limits_.torque.x; + } + if (limits_.torque.y > 0.0) + { + if (wrench_.wrench.torque.y > limits_.torque.y) + wrench_.wrench.torque.y = limits_.torque.y; + if (wrench_.wrench.torque.y < -limits_.torque.y) + wrench_.wrench.torque.y = -limits_.torque.y; + } + if (limits_.torque.z > 0.0) + { + if (wrench_.wrench.torque.z > limits_.torque.z) + wrench_.wrench.torque.z = limits_.torque.z; + if (wrench_.wrench.torque.z < -limits_.torque.z) + wrench_.wrench.torque.z = -limits_.torque.z; + } + + ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force: [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]"); + ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque: [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]"); + } + else + { + reset(); + } + + // set wrench output + wrench_.header.stamp = time; + wrench_.header.frame_id = base_link_frame_; + // std::cout << "twist_controller:update" << std::endl; //added by zhangshuai@2018.12.18 + // std::cout<<"wrench_.wrench.force.x:"<setCommand(wrench_.wrench); + +#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 + gettimeofday(&tv, NULL); + num++; + twist_controller_update_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + if (num == 9999) + { + std::cout << "twist_controller_update_time: " << twist_controller_update_time << "num:" << num << std::endl; + } +#endif + } + +private: + PoseHandlePtr pose_; + TwistHandlePtr twist_; + AccelerationHandlePtr acceleration_; + TwistCommandHandlePtr twist_input_; + WrenchCommandHandlePtr wrench_output_; + + ros::NodeHandle node_handle_; + ros::Subscriber twist_subscriber_; + ros::Subscriber cmd_vel_subscriber_; + ros::ServiceServer engage_service_server_; + ros::ServiceServer shutdown_service_server_; + + geometry_msgs::TwistStamped command_; + geometry_msgs::WrenchStamped wrench_; + bool command_given_in_stabilized_frame_; + std::string base_link_frame_; + + struct + { + struct + { + PID x; + PID y; + PID z; + } linear, angular; + } pid_; + + geometry_msgs::Wrench limits_; + bool auto_engage_; + double load_factor_limit; + double mass_; + double inertia_[3]; + + bool motors_running_; + double linear_z_control_error_; + boost::mutex command_mutex_; +}; + +} // namespace hector_quadrotor_controller + +#include +PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst new file mode 100644 index 0000000..790bab8 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst @@ -0,0 +1,20 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_controller_gazebo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ + +0.3.3 (2014-09-01) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ +* New controller implementation using ros_control +* Contributors: Johannes Meyer diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt new file mode 100644 index 0000000..1760b1a --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_controller_gazebo) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + gazebo_ros_control + hector_quadrotor_controller +) +include_directories(include ${catkin_INCLUDE_DIRS}) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +# Depend on system install of Gazebo +find_package(gazebo REQUIRED) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") +link_directories(${GAZEBO_LIBRARY_DIRS}) +include_directories(${GAZEBO_INCLUDE_DIRS}) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES hector_quadrotor_controller_gazebo + CATKIN_DEPENDS gazebo_ros_control hector_quadrotor_controller +# DEPENDS system_lib +) + +########### +## Build ## +########### + +add_library(hector_quadrotor_controller_gazebo + src/quadrotor_hardware_gazebo.cpp +) +target_link_libraries(hector_quadrotor_controller_gazebo + ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS hector_quadrotor_controller_gazebo + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + quadrotor_controller_gazebo.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_hector_quadrotor_controller_gazebo.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller/quadrotor_hardware_gazebo.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller/quadrotor_hardware_gazebo.h new file mode 100644 index 0000000..94b763a --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller/quadrotor_hardware_gazebo.h @@ -0,0 +1,109 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H +#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H + +#include +#include + +#include +#include +#include + +#include +#include + +namespace hector_quadrotor_controller_gazebo { + +using namespace hector_quadrotor_controller; +using namespace hardware_interface; +using namespace gazebo_ros_control; + +class QuadrotorHardwareSim : public RobotHWSim, public QuadrotorInterface +{ +public: + QuadrotorHardwareSim(); + virtual ~QuadrotorHardwareSim(); + + virtual const ros::Time &getTimestamp() { return header_.stamp; } + + virtual PoseHandlePtr getPose() { return PoseHandlePtr(new PoseHandle(this, &pose_)); } + virtual TwistHandlePtr getTwist() { return TwistHandlePtr(new TwistHandle(this, &twist_)); } + virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(new AccelerationHandle(this, &acceleration_)); } + virtual ImuHandlePtr getSensorImu() { return ImuHandlePtr(new ImuHandle(this, &imu_)); } + virtual MotorStatusHandlePtr getMotorStatus() { return MotorStatusHandlePtr(new MotorStatusHandle(this, &motor_status_)); } + + virtual bool getMassAndInertia(double &mass, double inertia[3]); + + virtual bool initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions); + + virtual void readSim(ros::Time time, ros::Duration period); + + virtual void writeSim(ros::Time time, ros::Duration period); + +private: + void stateCallback(const nav_msgs::OdometryConstPtr &state); + void imuCallback(const sensor_msgs::ImuConstPtr &imu); + void motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status); + +protected: + std_msgs::Header header_; + Pose pose_; + Twist twist_; + Vector3 acceleration_; + Imu imu_; + MotorStatus motor_status_; + std::string base_link_frame_, world_frame_; + + WrenchCommandHandlePtr wrench_output_; + MotorCommandHandlePtr motor_output_; + + gazebo::physics::ModelPtr model_; + gazebo::physics::LinkPtr link_; + gazebo::physics::PhysicsEnginePtr physics_; + + gazebo::math::Pose gz_pose_; + gazebo::math::Vector3 gz_velocity_, gz_acceleration_, gz_angular_velocity_; + + ros::CallbackQueue callback_queue_; + ros::Subscriber subscriber_state_; + ros::Subscriber subscriber_imu_; + ros::Subscriber subscriber_motor_status_; + ros::Publisher publisher_wrench_command_; + ros::Publisher publisher_motor_command_; +}; + +} // namespace hector_quadrotor_controller_gazebo + +#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml new file mode 100644 index 0000000..dbefdfb --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml @@ -0,0 +1,24 @@ + + + hector_quadrotor_controller_gazebo + 0.3.5 + The hector_quadrotor_controller_gazebo package implements the ros_control RobotHWSim interface for the quadrotor controller in package hector_quadrotor_controller. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_controller_gazebo + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + + catkin + gazebo_ros_control + hector_quadrotor_controller + gazebo_ros_control + hector_quadrotor_controller + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml new file mode 100644 index 0000000..b621e3c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml @@ -0,0 +1,7 @@ + + + + This class represents the quadrotor hardware in Gazebo for the gazebo_ros_control plugin. + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp new file mode 100644 index 0000000..466f65c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp @@ -0,0 +1,275 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include + +#include + +#ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.19 +#define _COUNT_TIME_HH_ + +#ifndef _WIN32 +#include +#endif + +#ifndef USE_COUNT_TIME +#define USE_COUNT_TIME +#endif + +#endif + +namespace hector_quadrotor_controller_gazebo +{ + +QuadrotorHardwareSim::QuadrotorHardwareSim() +{ + this->registerInterface(static_cast(this)); + + wrench_output_ = addInput("wrench"); + motor_output_ = addInput("motor"); +} + +QuadrotorHardwareSim::~QuadrotorHardwareSim() +{ +} + +bool QuadrotorHardwareSim::initSim( + const std::string &robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions) +{ + ros::NodeHandle param_nh(model_nh, "controller"); + + // store parent model pointer + model_ = parent_model; + link_ = model_->GetLink(); + physics_ = model_->GetWorld()->GetPhysicsEngine(); + + model_nh.param("world_frame", world_frame_, "world"); + model_nh.param("base_link_frame", base_link_frame_, "base_link"); + + // subscribe state + std::string state_topic; + param_nh.getParam("state_topic", state_topic); + if (!state_topic.empty()) + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create(state_topic, 1, boost::bind(&QuadrotorHardwareSim::stateCallback, this, _1), ros::VoidConstPtr(), &callback_queue_); + subscriber_state_ = model_nh.subscribe(ops); + + gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_state_.getTopic() << "' as state input for control" << std::endl; + } + else + { + gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as state input for control" << std::endl; + } + + // subscribe imu + std::string imu_topic; + param_nh.getParam("imu_topic", imu_topic); + if (!imu_topic.empty()) + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create(imu_topic, 1, boost::bind(&QuadrotorHardwareSim::imuCallback, this, _1), ros::VoidConstPtr(), &callback_queue_); + subscriber_imu_ = model_nh.subscribe(ops); + gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_imu_.getTopic() << "' as imu input for control" << std::endl; + } + else + { + gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as imu input for control" << std::endl; + } + + // subscribe motor_status + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create("motor_status", 1, boost::bind(&QuadrotorHardwareSim::motorStatusCallback, this, _1), ros::VoidConstPtr(), &callback_queue_); + subscriber_motor_status_ = model_nh.subscribe(ops); + } + + // publish wrench + { + ros::AdvertiseOptions ops = ros::AdvertiseOptions::create("command/wrench", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_); + publisher_wrench_command_ = model_nh.advertise(ops); + } + + // publish motor command + { + ros::AdvertiseOptions ops = ros::AdvertiseOptions::create("command/motor", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_); + publisher_motor_command_ = model_nh.advertise(ops); + } + + return true; +} + +bool QuadrotorHardwareSim::getMassAndInertia(double &mass, double inertia[3]) +{ + if (!link_) + return false; + mass = link_->GetInertial()->GetMass(); + gazebo::math::Vector3 Inertia = link_->GetInertial()->GetPrincipalMoments(); + inertia[0] = Inertia.x; + inertia[1] = Inertia.y; + inertia[2] = Inertia.z; + return true; +} + +void QuadrotorHardwareSim::stateCallback(const nav_msgs::OdometryConstPtr &state) +{ + // calculate acceleration + if (!header_.stamp.isZero() && !state->header.stamp.isZero()) + { + const double acceleration_time_constant = 0.1; + double dt((state->header.stamp - header_.stamp).toSec()); + if (dt > 0.0) + { + acceleration_.x = ((state->twist.twist.linear.x - twist_.linear.x) + acceleration_time_constant * acceleration_.x) / (dt + acceleration_time_constant); + acceleration_.y = ((state->twist.twist.linear.y - twist_.linear.y) + acceleration_time_constant * acceleration_.y) / (dt + acceleration_time_constant); + acceleration_.z = ((state->twist.twist.linear.z - twist_.linear.z) + acceleration_time_constant * acceleration_.z) / (dt + acceleration_time_constant); + } + } + + header_ = state->header; + pose_ = state->pose.pose; + twist_ = state->twist.twist; +} + +void QuadrotorHardwareSim::imuCallback(const sensor_msgs::ImuConstPtr &imu) +{ + imu_ = *imu; +} + +void QuadrotorHardwareSim::motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status) +{ + motor_status_ = *motor_status; +} + +void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period) +{ + // call all available subscriber callbacks now + callback_queue_.callAvailable(); + + // read state from Gazebo + const double acceleration_time_constant = 0.1; + gz_pose_ = link_->GetWorldPose(); + gz_acceleration_ = ((link_->GetWorldLinearVel() - gz_velocity_) + acceleration_time_constant * gz_acceleration_) / (period.toSec() + acceleration_time_constant); + gz_velocity_ = link_->GetWorldLinearVel(); + gz_angular_velocity_ = link_->GetWorldAngularVel(); + + if (!subscriber_state_) + { + header_.frame_id = world_frame_; + header_.stamp = time; + pose_.position.x = gz_pose_.pos.x; + pose_.position.y = gz_pose_.pos.y; + pose_.position.z = gz_pose_.pos.z; + pose_.orientation.w = gz_pose_.rot.w; + pose_.orientation.x = gz_pose_.rot.x; + pose_.orientation.y = gz_pose_.rot.y; + pose_.orientation.z = gz_pose_.rot.z; + twist_.linear.x = gz_velocity_.x; + twist_.linear.y = gz_velocity_.y; + twist_.linear.z = gz_velocity_.z; + twist_.angular.x = gz_angular_velocity_.x; + twist_.angular.y = gz_angular_velocity_.y; + twist_.angular.z = gz_angular_velocity_.z; + acceleration_.x = gz_acceleration_.x; + acceleration_.y = gz_acceleration_.y; + acceleration_.z = gz_acceleration_.z; + } + + if (!subscriber_imu_) + { + imu_.orientation.w = gz_pose_.rot.w; + imu_.orientation.x = gz_pose_.rot.x; + imu_.orientation.y = gz_pose_.rot.y; + imu_.orientation.z = gz_pose_.rot.z; + + gazebo::math::Vector3 gz_angular_velocity_body = gz_pose_.rot.RotateVectorReverse(gz_angular_velocity_); + imu_.angular_velocity.x = gz_angular_velocity_body.x; + imu_.angular_velocity.y = gz_angular_velocity_body.y; + imu_.angular_velocity.z = gz_angular_velocity_body.z; + + gazebo::math::Vector3 gz_linear_acceleration_body = gz_pose_.rot.RotateVectorReverse(gz_acceleration_ - physics_->GetGravity()); + imu_.linear_acceleration.x = gz_linear_acceleration_body.x; + imu_.linear_acceleration.y = gz_linear_acceleration_body.y; + imu_.linear_acceleration.z = gz_linear_acceleration_body.z; + } +} + +#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 +double QuadrotorHardwareSim_writeSim_time = 0.0; +int num = 0; +#endif + +void QuadrotorHardwareSim::writeSim(ros::Time time, ros::Duration period) +{ +#ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19 + struct timeval tv; + double start_time, cur_time, tmp_time; + gettimeofday(&tv, NULL); + start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +#endif + bool result_written = false; + + if (motor_output_->connected() && motor_output_->enabled()) + { + publisher_motor_command_.publish(motor_output_->getCommand()); + result_written = true; + } + + if (wrench_output_->connected() && wrench_output_->enabled()) + { + geometry_msgs::WrenchStamped wrench; + wrench.header.stamp = time; + wrench.header.frame_id = base_link_frame_; + wrench.wrench = wrench_output_->getCommand(); + publisher_wrench_command_.publish(wrench); + + if (!result_written) + { + // std::cout << "-----------QuadrotorHardwareSim------------" << std::endl; //added by zhangshuai@2018.12.20 + gazebo::math::Vector3 force(wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.force.z); + gazebo::math::Vector3 torque(wrench.wrench.torque.x, wrench.wrench.torque.y, wrench.wrench.torque.z); + link_->AddRelativeForce(force); + link_->AddRelativeTorque(torque - link_->GetInertial()->GetCoG().Cross(force)); + } + } +#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 + // gettimeofday(&tv, NULL); + // QuadrotorHardwareSim_writeSim_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + // num++; + // if (num == 100000) + // { + // std::cout << "QuadrotorHardwareSim_writeSim_time: " << QuadrotorHardwareSim_writeSim_time << "num:" << num << std::endl; + // } +#endif +} + +} // namespace hector_quadrotor_controller_gazebo + +#include +PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller_gazebo::QuadrotorHardwareSim, gazebo_ros_control::RobotHWSim) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CHANGELOG.rst new file mode 100644 index 0000000..44dfe6c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CHANGELOG.rst @@ -0,0 +1,25 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_demo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ + +0.3.3 (2014-09-01) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ +* updated demo launch files +* updated rviz configs +* Contributors: Johannes Meyer + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CMakeLists.txt new file mode 100644 index 0000000..3923701 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_demo) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package() + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS @{name} @{name}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY rviz_cfg DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/indoor_slam_gazebo.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/indoor_slam_gazebo.launch new file mode 100644 index 0000000..283f1c6 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/indoor_slam_gazebo.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch new file mode 100644 index 0000000..157f8cb --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/package.xml new file mode 100644 index 0000000..3d1e9f8 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/package.xml @@ -0,0 +1,28 @@ + + hector_quadrotor_demo + 0.3.5 + hector_quadrotor_demo contains various launch files and needed dependencies for demonstration of the hector_quadrotor stack (indoor/outdoor flight in gazebo etc.) + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_demo + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Stefan Kohlbrecher + + + catkin + + + + + hector_quadrotor_gazebo + hector_gazebo_worlds + hector_mapping + hector_geotiff + hector_trajectory_server + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/indoor_slam.rviz b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/indoor_slam.rviz new file mode 100644 index 0000000..8d1d818 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/indoor_slam.rviz @@ -0,0 +1,207 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 460 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + laser0_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2 + Min Value: 0 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /front_cam/camera/image + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Value: true + Visibility: + Grid: false + LaserScan: false + Map: false + Path: true + RobotModel: false + Value: true + Zoom Factor: 1 + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 0 + Enabled: true + Name: Path + Topic: /trajectory + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.23194 + Focal Point: + X: 1.66903 + Y: -0.217464 + Z: -0.674569 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.625399 + Target Frame: base_stabilized + Value: Orbit (rviz) + Yaw: 2.89041 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1026 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000012e0000035ffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000025b000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002a2000000fe0000001600fffffffb0000000a0049006d00610067006501000002d9000000c70000000000000000000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000410000035f000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065f0000003efc0100000002fb0000000800540069006d006501000000000000065f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000052b0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1631 + X: 49 + Y: 24 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/outdoor_flight.rviz b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/outdoor_flight.rviz new file mode 100644 index 0000000..4d25080 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_demo/rviz_cfg/outdoor_flight.rviz @@ -0,0 +1,192 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LaserScan1/Autocompute Value Bounds1 + - /Camera1/Visibility1 + Splitter Ratio: 0.5 + Tree Height: 408 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + laser0_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2 + Min Value: 0 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 10 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /front_cam/camera/image + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Value: true + Visibility: + Grid: false + LaserScan: true + RobotModel: false + Value: true + Zoom Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 28.8231 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.630398 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.16541 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1026 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000012e0000035ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000227000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026e000001320000001600ffffff000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065f0000003efc0100000002fb0000000800540069006d006501000000000000065f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000052b0000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1631 + X: 49 + Y: 24 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/CHANGELOG.rst new file mode 100644 index 0000000..eef8dd4 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/CHANGELOG.rst @@ -0,0 +1,22 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_description +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ + +0.3.3 (2014-09-01) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/CMakeLists.txt new file mode 100644 index 0000000..a40f4f5 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_description) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package() + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS @{name} @{name}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/launch/xacrodisplay_quadrotor_base.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/launch/xacrodisplay_quadrotor_base.launch new file mode 100644 index 0000000..3da01aa --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/launch/xacrodisplay_quadrotor_base.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.gazebo.xacro new file mode 100644 index 0000000..a69a4eb --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.gazebo.xacro @@ -0,0 +1,6 @@ + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.urdf.xacro new file mode 100644 index 0000000..284b929 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box.urdf.xacro @@ -0,0 +1,12 @@ + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box_base.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box_base.urdf.xacro new file mode 100644 index 0000000..6b67ab7 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/box_base.urdf.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/blender/quadrotor_base.blend b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/blender/quadrotor_base.blend new file mode 100644 index 0000000000000000000000000000000000000000..af035a60e89bbae923a7f634307459307b5954f2 GIT binary patch literal 385148 zcmeFa3w#}Ao&P^4Nt;j#Ed?|bgm4ie7YkG*AZ@NK0Si=GAR-V#drBg05|b2zf29#` z@dhHO@m|48e(UZkif&YB3v}1`6BSf6>w3ZUmc?6j{cD7U{GMk%&*yw*o--%S$;qj7 z&GePdyw5z}=XvHkGw+!*=ggdjHLEUeT6O7ZH=g<8GcGlgRY4Fue&y+hOl~t1W=Ofe zC(U@>hID7zzUU3-ueqe*^s*qh_Hx91&&;=RRaI3lGA{=<)^KX#r|S>sHT70LcGLnp z4i*O`165_gS)Vr3W?0tVwI#i*w=cb^t#jF{yLxWu?QYwU?p=1>=5*)AbkDL^ZEf4w z)75GUboVYhS68*PH)%eb}x)ZmM5jZxhVryDdk)uYC&jF-P<`yV$#- zC!H=J`>EiF;O6pHd#EtonHT)7bY8gLE1z5#F5l=O$Rpch-iKcE%5Z+mhnvFXtA%(@N6Xf%Jt$nx!C`Di71yHw{l_009!L8>a#A=ReQ|RCGRIq2>HKKr z)7ryg0fF6as?1C5*P~|I_Iv1J^wUfx4vb?t##MX;>zDN@_{VlP$+X9EjH|wEb+|rQ zALI6v#p76xaqWFBkFH(CVT%1^ImQhx!~V_s7#9Q&mDuHH9Lq7TzXoxvk8yj-;&Cj; zxWRki5!T1J%5YDKwCBn(Zs=c;Pu8cnczZ0zxXMFc8s?MjD(<{+v*hxz9OGJQv3#tL zaeK<*aV*EUiC@64tdDWVNZ};gV>!l+EyH%k`WRPP7LQ{&#*O~lx^VeeALDkV;&Cj; zxWS6c!Z_BaxOhHUj&T!j#Qw?p7#9R(v3#-|<0>y~$9iz>GHzEY9>;QwqaRow<0{L} zi|t1&$GA*6*Dl$tzf4@Ud6C}vwwbmI(Elmq>db$7RYMxj_X68zdB%@DK`-RWAImeozZ3muy^PP5KbB|w zLg zn17aMeD!az{j*-i<;p+HGk)Z&>2Ud3FZ<7W{Qk2%+v8{ZM{(Y6Cc}a4^EjC6cxDUlG3S80&C%JO>6VxOq3lR^ z9$${C!v9AGP(R1b+rTv^ABpR4k+MmX;dWy>%DBf&A=~f7_<=i{P{$+n;kfD}amLJ? z|J&uU!x7>CHg?q1!|_0E5ZFKazPYrtELa#U2>wvEH2ez#vi@U!eiWX;#An+`yCey- z;spyN=8ySe{#Y&Z8uFektIRU);=IX8%R$K9W~IK~{4Fsr*!Qt`Q|OfQO?L7TtrrfKdc+;1k1!1*!vCmwGz z{&VkRcs|13A7QpJZf7qs3tm<>7Mz#L&cH||>gTxTJ=kxC-y7dwS+76Ntalg3^`0Dm zXCvYtslV=|6JKoJ`()U!H;KRRUc`QLg^9KM4aZ6Q%~-HBuz%SD^K+a@iOr8cAWCcB zpFdm7{4rn5AM5A(0NZ@6*v)yV;QYo^aQj1+ziR%*+>B%0@~m`Y?l=GD&3DL=bxz#^ z6O#(c?fzFb-_&?q>Z|6DMKosq>?>?fb8TOeqM{{|rzxQn*zI14}M?l=WVMlwdw8&Kj8A`?~MzNpHZvPUcJTSdqwnx%#k7X;w%I6 z&HOUol?$+ca6S0VDqox1Zc6v``t$e3g~qR$QLE8jb(wr)Fo&{mDDDv$Z7|==FY`Te zAo9)i;JTCo#xg@e57Cg~qRI+tR%`y|L<|u=T2j{93miJI>@g z2=mQ6&zqvZvo(wPW`3FPp~c8I*Mr}l^3~hkwlS|wJFKeE_;oXCHM$fF!hG9TnD6*! zanZ7~H_d!Azsz^d66Blf!EaId>Q47;>gw6j*0~|+`#*jhxzPBvGisHs+wmWt_K%n4 z(b+|sX1+U8^YX!w=aXZ_D#`8gu5&zg|T@LZRyE6gj47TIrZWM_Ev zd8nV`@ulFFqcF}?mSe*9yV_LCI9^9gjIVk}6RLTn{-iH{epo$ugCxA^6@&XW&8OBt|r-BH8%vWeRs1Rf8*hY&C6Bw2JBE`ik4NF+^qyB z{%`BExwYGuKTI9zm!3J>?azL9klXKlW;%N-&#r$__Dj#oSB`PZLqE_589yA4{gmFw zd_KePH@VM=`MEC_lGo8!m}I@gT;M7#9SbHCGB>^s*H<|nI|l7FADf>a7{~29F@E4r zZhL+feNJO?WAoz=`}%Vl=8O4bf0);h2i|G&_ooLrFEF3em^(@yvTWa@+0SY0{{M6H zX9s>RLqAxK&JPP^{=?^7OH3~8=fgp;F#Me67p7)=uH+BV?d#aY{QbfYDGJ;DrqUjt z+;;p0)5>)GU>0z{99o`C{(kaw7RKhq=F5I}p7s3NOxxEFc7G$sVZXQK9yhO%pK$Zd z$PB5>{YQs}B)0FP?5CUjzVzWCMebK}^P7L??q70jMw!9?bhMa!xM?-o-;9Tycj@8& zGmsbhncq|5_f`0PAD*vqVIDBawa3wyn-?B5`$;NwMae6}Z#uIC#-qznKgVMyfQL@T zI9@uNY`^V!8sq4Z<`Wa*>t5A_8r=6xb{!t)?CN8$T<-Sw@H}n6#+vgqjuX$*t|(a= z*uU%{nxCZWzKGg)=V{Cr^EYx5^2fYl2^KX5b*p!4X%;n3ZUf(DUfgc}tY=SglDXAP z+kqao9GxG|)PMNA+_3w+JeY54Jhk+;U}=K1Z2s(DuPJZ0FMmh{Yt5_k%x5m9P!R`i zf6N#2SN#Iyk9kEG7B>dLHy>noezi#)&EKnCfn53HeAeT}e*VmHY76Z6M>F@N0unOF4h_{L!1+Q{}NzS|VI*ZkS->khZQ+=Zab_GdXd z-_7R8&!5@;?hWoNy*s!iVf(Xgvwyj_KfB$PnSc9c`(wVCKQ0vWN`AJC>Azh0yQ0MI zzr`CWPR0Idx4(X~e#!QBVyv-u*zF-UjDG5!S+5;+(7?_huzwgX{QjC9cB<+3kFH#P z&o_3AeB@XpZz`lWFGUF%92i@A}z})9-!R zPSb~-Td&+!dr;3SRxYjmlI_FF+wa|SU&kGWE~J>W~5>rEf(H|*KD zva|29+PW{THtk<}@997K(vDM4TfXwB11~Y{*L=Ql^VSFd<>9gVjt|Xy@VkHc=&oO{ z>}hz(Gmky+k)M_}oL#ZKuF&N#wEqk5euc?He3TjM(qB0tw|uy2mjF& znod4#ndQruhxbq(t0xcyA<$cr(K)|WKR^9RQ9_&2sZA-?YAO{l}2{|Aq|9Fvn; z&}(*i?11zCpO{#C{_n4p{IF2wKjd%0d|rpK-dw@NnGk^9v$HIlXQae(9tLBXSEL^Tl2Y>x|=ml^nmx^ZF;Or|N=Wdhd@e za2wonZTkZsH?j8pksBwzKe{kg8Q8z{HK{e=8O4b zP0TC#*)opiFUjk6Yb)k3bS}0}n?HMfmCWm_HIoO`WwrnFjajdKpgw+mwK;WBc&+kB z?^#xR`OSCjsQAQ5;d}1otIw_d=~EwF`JVMh?X>mosyXAnv(HM`zUVJM3$L$UdE!6q z2!6b4$It;UdcYnde`4a#HZj{xIa|KP)b$G!Z(}T%n|j}C>RRsNJJY|fd*8}Oc3nLA z4|O(v+1DPdU2(zfJOA|LNjps+cJBE2H)|hVa9i!YA3v(r^r80g_wT-M@b_!#7Ht0c z4%3GnuRFYM=gqJB$DLzmoKbK3Q2(CQA6Z%X#HUv7`rb#s5(M+Vu=~C*+<*S>KDT1q zX#=Le|G9j)|L_NIm{-#9^|w6sz#ZTF=&pNCJhb7p|1|l)wdrry4^(fjD|Gn_?f<6h z+e{wf!*pM>`TN6jnrkn<>(4t*skm}S$?=~y>lE+dSmV<>2V>V)j}GRpuQE3n$FFPR z8sanwMk4x=S0owsd#(cJ$eMmad=CqXBt@j8@!I4?kC&k8!^n z53dDRz6|4$AL2Pb(O5g}pNGx&3*^amyR0%X?AN1a+V*?Y_0voy4vb?t#?>^S|Ey2J z^UOOVoX=I6G0QP-*u}9vThUk8xG=<8dsQV>#AGu5vq)jjJ{KI`T7oGYH? z89(UaSugwTZ>MaZ!JmYzO!g`tiTu7bNSGgx90-CHt&V)nM8T{w(_Lst_`1p=c{pCaj9Eh>*DRQeSaLTcXOa% z9IsdOC&gFY*@Sp^edqDt$JwvkW1icDpNH?VvF3UY$BEZ_wv{gp>|gT0{Fn=_Sp%Z> z-F1BCi}_=RnAeb>h0k5H+ZN7L*<*}xz06K0Tuh4>Er__^a6jRGGxl<9zucd=ztSH( z{?JG4#Ifdlh}+pK%!`W_U6Z;hJQK1d!c)Pp=tv9d=XmTgaPt-U?Kh0$dQXfW==JuK z&zNhSc9`CE(yLv!@ja4%HnH|PsT(KmH`kc&kl4TMA)24CKCcL(_T7HNd@+CQ4)ba* zC+{+A)a9$W;mqY1KJbA@-;iOJkB*tIL)?p&mze4LknMGlz1{D3O)BwXh>2(ZY;NuL zT-EZtQ0dBwcqNKL&+;#7-_xsTY89#gt*BR&ydV%MMxz1C${;oDD`ma*+ zrH!&{Qtu4kImdHWy%8SZc=Sqe%~kpNfpOfvljHSVM)cxu?}tZBZft)1fqt0Iw&us? z!#poxzL-DO!MuiST{v*nZqA$B_Q}S{o#2a1N3U_6?QzRbZ>9q8D9xr}R4f4k>0GQZ6C;C0A1 z*F)}n@4wGwq_51WdpH#L3>|H7`(}Qb@3H&baoBC=u1lGbw(tG-xr}R4U(YO$otTlp zV>Rp@m~ZBn`R?C=d~-ecH7Vcw?{gW~r2cl#Wn_Mt@5)`sH`hb%eDA-{Wwbsy%q$pv zGS6SgA~3ga=9l>%c>ww5dhnap_Pzf;m(llkp`V5NIZ9&YoB3tFTOLBbxgPvj<$M2q zE~B4s_YteVsI$tl1rl0I&buW2W6@qA6SDInLVVWqepWZ3{^@;A!{^T6a~d0KKc{iy#LsDtEL|Gdzw9BJpQO)e5Vh|XLD<}FMmh{J?7PY_q-qGi}|bk7V^iuqJMh6kLADR^M1Jf z+59DKpZI)u-#zb#`C|UK|1+=1pLxE|uFbnKZT~kMj67tg?lyn-nm_wF;OpJ?aF?03 z13w3_9GxG|)PMLKz_6Y-b#HJ>>h7RBVf(Ufvwyj_KO3~~?w4l1m_IHS^GbfUjHBCM z(*Ap7soj5zH%vT={nH*l?ENTY-j8y1`j_E-AJHBh{iavfMcGbIbwA2wQ{0cT)7)qB zVsmfFnUVWZp7~Ms{V4Xn72D1grmmlx`%!AV`%xIr`%x}(_oJLV|0A^@)q zZSF_e_4%_G4d4H@8$YpPXwepPKg!VZ@0j~h-evAbnLqg00}q({QU1f+kFwj`kMaR? zKgvJ3`%wy6{zCh|^2zsRt-tBM7QG+kRC7N{sktA;f8Nj9GydgVyIwdPHg`V?QxT5+ z=Otlh#{DQ8&HX6H?B#wGo)`XazaPb@#~g2-JA_-uTFU0XU%`O^CUNJ8FS~I|Bi9AzNBfM|1*xqztN=lfd=n< zf49d;E$B7Ap9#;`AAbnv{~RZt|5wdl8rZ+&f%*Al!jTS9`|kXo`C|UqW9Bso@3Rlg zGTZy>gQkIp{{5j{Ki%AP?VhjHtz7(c<07-J?Y#>2{Hmb@(FYs01JA!KYd)Uu69iAL zwtv5Q*~p3o-#@et*IMjeVLz^@n%`KmX#N?lKKo*Hu=)Fx+rFZ&+i4r=zg7&k+u!M? z58RILH;p8$&a9^W`^vKl6!8U{=jm4O-`8d17JRcU>)4vBzUkz`#@ak?H)HyS{;~7y zKri{<&*pUqUY9^={BNJ$Z3laPIe)&rb0@nu?Y~+V{vImF!#{#|1|QGQKaAu4Q~8sm z`0qEON4(xpZ}ulUkU_cr=kkR9*;Qv^?f0wQIMF|4=Gue(OCDnNc|{Pl@BG7jF@MZI z^Xhf)S%Z6f|F^H6H_Uu7e@qbb3LbFVpB=w`XD|@d1;NH0!N7;6wLh1+{b{z|nc3|x zSN_V1{Qi&m3C|ynHGXj(_@bGSZ3&jv*d5yRLS$-VWZJ*}8krTD+*5Di1Mh!-_-q0j zZ>OX6L<-MM&ShXO19KUe%fMU)<}xssfw>IKWneA?a~YV+z+49AGVphr0ej72?s~!B z>B^s5?70lgWneA?a~YV+z+49AGBB5cxeUx@U@il58JNpJ9vPVW`hov^c~s2jnDe>i ze2#g|llZ+UKHuE$#=1+J(wo}0ZtnAEx9`Dt>ZoggZ+LYtj^!9v@e8!a`V@yLw#Rae>vwUik8!4lWp?=)$8wAtcX6zb zaW|L8<5-SyEx$xRSs&vzx)sRwSdMX{E{^pnF5Vu?F|OuUXpi+Nt|k25DcfT?#tpkT z*2lP;%j0n@$2e}!tdDW#w-3Wfw#RaeV>#AG-Z)=z_POGNZa?|9nYQa^!m*hQ2W~h1 zc3|7J@hs2y_WweDSTE!K^~!jbXZ)CpXT5CKU%!lJdB!(Cf%aK1+szfv@{AvG@vN8o zo4=i~eU@i@^{>%B>*aosE1u;UKjh+BFZ=IrZ)~6C8DH@mw9k6E+_~aep7H%Ip7pZd z{&vguS)TFZE}r$W-CXf3&-j+#qW`Ry{r9(jw$Jj6A9eApm-){X&+?3~`5oG4z3g|c zc$R1Uu#0EC+0yZS!+Yc*YQK)IL|c&Kv>4gIG9gY3%#3)dwJd@u+MX|86?WSM^(x zpGV33F<;CdYhqqQ4xb@zr%ta2|F$U{*S)E!)2!=l?twoqaLd!>_>izr5Nz@0yO-Sv z_o0-S1vWpRZhzjdY=Noq8dAuBL{}IP1O_yZ_!c|IF|&yGpyRtWPJ* zy$4C_%SPH;&!XF3bh`TBy+7v^u^rjTzoxw*~! zEip6y;_lC2zUV*8(fOHr!U_0~zYkny7Jj~|aia9L`M-4aMawgP_BFN_oo^$f>`yl@ zoW1e=OKKroc%I?^9UR~ZCuU-#j4B-WTV=T>}weuaJWShh6Pl6lTW zMKDr=^PR!dQ9s8+N8ma_#gWO^8%F23&re?d(y(7VUt}E57n9@P*@*t}zH*=0`{eNv z?nElQ-Z1bEQzk1M{=eEk158Jky3X=< z=r454^KHA;*@5#xng8*A&y>;cjVw*AEIGjZncw$(*+CWdx|G?h%+u?Ffcdl0N%h&t zCOb37FjktJzy71J-HpG{ZI=(=I!%A;ap83_zdD;g`+FmdesAPFe<6#&nS8ta zGT$SALB6>j{HB%f{rC4q7CT>+xrK}W+Ka!$v%}0c^UHj<{MF^#tp~p~<$M4Ay^+P< z?~Q~Heef4_mYMlxewpuyCy{Ti2ftb6d;k5tk;SR_?~T|4aIpvGoB3tFhyDlo=6dk^ zp?vSZzc*ria+p~#`lQZZ$RaTF&HOUoHP0a5Tn~QJ%J=^Jdn3NT>-~i+0yE#tFY`S% ziF|WC_)RO{`|t0K`1x+|7qSS%p&I`QBfDZ{&oh7pLAF zTdY+vDh@E;%rEoZG9UTodhlCUzW3MP8(C=bjiuuG$!dQgi@?k`^UHislq26<4}R0@ z8dqJss&(!9*4_>2&e$i3e#vAmIDTf$Mmp0uS&_Ot!a;CWjEV!yH}lJU4;_Gfb3OR2 zXY#eRv!l+)h%bTZ-ccXW1M)66&X%Y2V5M83Hm z{7N(V>TbWOw_`)zn_8jq=l8VT*wJ^B->p3OVdt2)FEqX}Y_@8xX*S){*OAVrZ7j>v|HIE4cz*H{e<6#&%s2DPe2*N0 zd~-caRDUo0e2QJTsDAybOE0KjV^2{wbanQn`}ZKmt`mP=zRY|xzsz^bp~yGaL;ESY^WEK(zOf_y>b#nc=6fdb zYiH1Ayn`P$=O=^iCFJ|$c!8o2^UeG+-xG%+-&_yPr{~W1#*R&!w)WOyZkn z&}KY=t4+Rn#T)rPC0?K?#C$Wq%=gf9kZ-Psku!7WySF>NVe96$o_snO&G$^=*Ug~K zc>n&x952_omyqvM;{}RB%s2DPeAiSW-&_yXXHAjs_O^}Y96Yc7#qvFa_;oXgiFdF! zJU_u}$oFaS0!1O_oB3tF$Bsb0xgLhj&YkbRp0>{3?)Pfu4~HVJ}gzGoCagEr%N z++)rM`FR?)?|t#TaORu&Wxgw)hkSEAG*7hUJRj`s?Ts(WG$@+ynZ&Q#dR_0&3rSz z%=g%d$T!!6-y-uhlWQNgu0rDrzkX6_^3CffSi0x?3t0qazL{U zu31l^@oVzjpV+R(mp!vMWq%b~!-<+K+7$E6{4(E_CnMin4}J%jubCX{Y+Z%M7k<2a zhxvT|p4eiY6r&#d5Sl61z#$ITA;m6CqtnWiq`wLkFX11 zo9n@Eg84GHr}z5vro)BCubE-H>e~S(-|rKesbgYP9ALhgU*>!0bmW`s!Ec@U%6GeF zJ%z?M=D9zyU5!8Z%o6YWK2@4ov?=DB`DMOq&P2Ys9{di(^VPbsYm;AMYA!fF-QCw- z)wRhyC?dT%&ucg6*=m#T_#HdPPTl2Mnag}Lzsz^_Gq`V+>%ni#&sRrpx4Ci3FO{1M zj?cHjtgd|S=_RSx#$G)xM#TZ(7GvCZF^WC1p{jyvSe$!W6a_O3;=o@nLjF)`&>q6uG{^ZXi=KB!wckuYE zC;yuAiD15&U*@}_6#3?Q@axRvYld&=3yq(R_w)Kb1eWe_e<6#&%s2DPd=Hl)-&_xV z)0uqbxm~lKg5&eso!D$-{9f~Yh*w592%Zz8;sEo_{4(Fo^O0|^2fy`9zS_22*OBh* zYwg;wVe{5r^SOgxGMNjFuSc_0m*m%Mq%&8Ve8<19`P|6dVofsN%rEmjUXFZoJ^1Zs z@|Etqv7@J}bBoRS=8o&z`=<9fVxjS?)NIw7={Fl$jH9#iT^X5MtV!mZ`DMNb4?w=T z9{lz*`ReH0*wK^TkXN!zWrfCH;F_IbvP}=ZX1))xBKG~*Bm9Ld0yE#tFY{e}Ao9)i z;5VJg*Y$0E?dd$f3}GrOH2(aVH5*xsGflp?Mc6!#B}T;o=9~FtzDE}#-&_xV>zRCQ z>FAxw{Un9PUo^93Ba5-r|Tw{Y<{h?Uy~h{@lHD zq4Dcx)@-C>C0Y4CDl)fNlgu~s%Y0WXM!vZo{Pr{Xie3Qs3r)d=#uxtn@`=dzPteil zP0`=kn#FuGzs&dWA;>q^gWsNwi@v?wlJ0Hy%S^?E#!prE^yX#1zg#%Iz=BFK-^?%b z-FztW&Gq0n!hCgh^=xU|oWD>CjlXz??P@tTnfLSYJ9ZAz%%V*(-^?%bJ$@MS&Gq1S zfcdgFl5Fisw}y|$^$ShGg~qQ(yH)G#V|k}&C--9ZdTl2bKs?hk$!*;9A&$nGI#}g*sYlLQMQH%t|xc`#* zWxlH`k#DXCwT>y~%X~iHoo+QZz2>QuLgUTn^WEtxbJuI0?P@vRoR#mzni-oaM!uO} z=6m!A|*VY?bol^V~Q*)v5mt9_UQCnVnl;58hn0&|YFXnS5{gTDVH}lJU zw;zdob3OQdV7{DBHgs+7%Ih?|(0J#Q#(dk=ay)F#zvKB{qM5O&V&t3oWxgw(hkSEA z#5!Pr`HG%?x8`wDSZI9V=im2+?|1BS@pyTtzmP>>yWIA9rpzz%J$y9s&Gq0now+}_ zZR^I4E`RRcxzPCfnKc{f*e8rn;@_`&ab#|>CYf*Mm-%k4LcX~k{Pr(txNueD`sh2J zuIpZDE))9Ya&e*Yx$2$%>)G#j+{c;IuVMxznQ!Ko`5r$O`R01?t5&}3jUBz4%-avY z$dp`Y{Q4QS8tu|a=KW6m3k9?3>1a#QY37^xWxfZGN4~ipqA_+_`P!J?+}GBcZ`KQq zZ?dgc6_WKuCf`^p`n%L$$RaTF&HOUo)z3%1xgPwcm9I>uTQ_%YxS`iCn1c(A&t$r4 zO&+aAJM$OgZ!q)CakSKIPcz@lFY`TmBJ$1k;J2%M^`tkq<$u3aq4Af_sMY9Fyw2qN zy66kxbJ=4r6akoT=9l?yuSUMP9{i@2ue{&NhgB6CU%(rAwcK8s9XdR-;StUUU9^NA!gNcTL4!nss2lnP296_!Q)u z>%nh0lCQ1ic}nIM-)VmpBCM*=_@)`P8eNJjO}^vXcSUq|k*1k%=9l?yJ`MTidhnZ9 zzB2x9HJ3bqvz=40f=@GF0B%Ksg3{vI}eZ_0#Zh7In-$3C5G(V~s>w?{HO z_`TET*@8o7p?;1>&IDJ#2;)p;Zle7_{JzlOT}|Qg*nU@=jxvtFi|gQ_tIsQ9qgI)G<$CWwG=}_nVOco5sy@t9 z%Wv^qd%pnl$9yq=EXTZ>pZMKneksg7y1coeij=%Bn!{#QCs(J%;1pLxaL?Hi28n@cb=D~9!(vP+L@4q==PQLJJ~kUxhCF>qv7vPFMs`O z@Vm+tH|F+3rRg8H-!GW@6S|w#2eW|t#n67=x^Vm9xZ=9i@MN!jfYsK`-P8W0L!Pmp z$FrVan`xV`)6AP5`&Y3B_lt$zeYf}tSH_IY_K^xIA3G}a_MwJLBYc?0hnsn}ewH_r zVfp6YA^vc${L$w3v!msYG4tabukV&@bu=Xh9x7ykMG3LB2Uf+C=%RR2}_yCU= zczmG83q3x_8jywu|(JwD3g=Xrdz z$H#bF8Sd8Pb z7{_BVj>lpgkHt71i*Y;_<9IBFZ^Sqri*Y;_<9IB_@mP%Gu^7i=F^8Sd8Pb_zZ9RKGWmn9>2)rvpjyW$7g$72UJU-9k z6&|njxYpx3kLx{d@VL?ACXZLenEvmdyddVsiaK0(;QWc7u8+;Hc@pniIKTg`F9_eC zkS9*Y^*M57kH^((WA*X<34O=o7kwv&@5Jz(7`_w3cVhTX4Bv_2J28AGhVR7iofy6o z!*^o%P7L3P;X5&WCx-9D@SPaG6T^36_)ZMpiQzjjd?$wQ#PFRMz7xZDV)#xB--+Qn zF?=V6@5Jz(7`_w3cVhTX4Bv_2J28AGhVR7iofy6o!*^o%P7L3P;X5&WCx-9D@SPaG z6T^36_)ZMpiQzjjd?$wQ#PFRMz7xZDV)#xB--+QnF?=V6@5Jz(7`_w3cj6%QAJ2#B z7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4 zei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc> z7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!LY)O}~iY7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc> z7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crA zh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ zix__4^*Z`R48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crA zh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ zix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2 zzlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+3$OpvFJkyb48Mrs7cu-IhF`?+ zix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2 zzlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY z@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb z48Mrs7cu-IhF|!3BK;zUU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc> z7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crA zh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkzG-%p@l#PEw4ei6ej zV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLd zB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97M zFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8 z{33>5#PEw4ei6ejV)#W2zwrA`^otmN5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2 zzlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY z@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb z48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XE0f0BL? z!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-% zG5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ z5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs z7cu-IhF`?+ix_?p!!Kg^MGU`);TJLd!tZ0#FJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^ zMGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKi zU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH z_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-I zhF^Gp2mK<3U&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY z@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb z48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5 z#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkzG_lwalV)#W2zlh-%G5jKiU&Qc> z7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crA zh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ zix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2 zzlh-%G5jKiUwHp1{UU~6#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs z7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4 zei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)%vkv(qnP_(crAh~XD8 z{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p z!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-% zG5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ z5yLNH_(crAh~XD8{33>5_5 z#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^ zMGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKi zU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@C%=}L%)dO z7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4 zei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc> z7=97MFJkyb48Mrs7cu-IhF`?+ix__4^SkI5G5jKiU&Qc>7=97MFJkyb48Mrs7cu-I zhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ej zV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLd zB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97M zFMJ*#{UU~6#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ zix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2 zzlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY z@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)%v67o}gs@QWCJ5yLNH_(crAh~XD8 z{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p z!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-% zG5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ z5yLNH_(crA@Oj1bix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5 z#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^ zMGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKi zU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJytoPH6*FJkyb48Mrs z7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4 zei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc> z7=97MFJkyb48Mrs7cu<8-&3Gp#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb z48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5 z#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^ zMGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zwq~2=oc~k zB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97M zFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8 z{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p z!!Kg^MGU`);TJLdB8FeY@QWCJ5yLP1y(jub48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc> z7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crA zh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ z3x7Y3ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^ zMGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKi zU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH z_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5o^cqoiNN@QWCJ5yLNH_(crAh~XD8 z{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p z!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-% zG5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ z5yLNH_(crA@b_)$7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crA zh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ zix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2 zzlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!P{3Z2CnEzlh-%G5jKi zU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH z_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-I zhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ej zV)#W2zlh-%G5jKiU&QcB%N>2;?^Dw+V)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs z7cu-IhF`?+ix_?p!!Kg^MGU`);TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4 zei6ejV)#W2zlh-%G5jKiU&Qc>7=97MFJkyb48Mrs7cu-IhF`?+ix_?p!!Kg^MGU`) z;TJLdB8FeY@QWCJ5yLNH_(crAh~XD8{33>5#PEw4ei6ejV)#W2zlh-%G5jKiU-~C6 zi1|efzlh-%G5jKiU&Qc>7=97MFJkybjQ&;Bt%~(ejQ)wyKQa0zM*qaH4VYhxW?AC9F-TKY2TfZ50>o>!0{bm^btL%yQPmKPF(LXWz zCr1Co=${z<6Qh4(^iS;iSG_itzsd}|{$<$pFT<{X8Fu~4uxd`j=tXzYOF2 zMg2j4n}2syYy%JgUAMTl>zUtho7G&*t&{QX@sowA*9!^n#}!tHkWSD0V%3yfQSiE-sP#w>5g z8|DYW$IQ5~%+!_IIREZa)>sh?{`=|kY?*E|%=PwQaY(VzF{CqikRv2}GS(1{>yrr2bJ2Cz<{&yj!THH;hhTw_po>TcDsD8sjz7q`0^pLLzS)$t*2Oy+#|GDjwV zLEVB+0Iv+uC*AE7KeLwsdVwv&7WnLgOz9+pAh{8=tzRTyy{H>Dz)yyQpS3Df~aj zY6|Dg)_=@5^UHj5eQ`Yu{b1^RZ%OyIr#E)=O}AfuzVnU0$h22TwvCzp=eFQ zwb=&xnEs`YD}M!_(9>bN*2lrs53vtF+q5|3Ki^cdE9&F#y8`sD?_mW%Gmo8E;S9+vc|3KC_jjd)oTCdRTm~<3i)tr8gV@S9SGNHKltoUB7vA5G-%1 zYTVk>JB^;aZ}MHi%UnO<D$x_FDC}$oa$Zj+-C4+Z4Fh z^=GgY6L&f0_Dbe_H=Cn`?e*T^t)+LHKNivWv*zs6o`2cxHR=4pMy?6U%)jJe=s)7y z4@(V>V|%Us3&tbm2ZhJ|imS8Fk8KS7$~bPX)lVQkQ|}Ms?P+_%nFGsBcU)-v`kuDV z-c4OSTc+D!R=L+cb9b=Wnc~E(xy5>c`DT8Z@6lf)-&_ylKgyNw>$}sP8#>Icnb)9v z&n*7@o^)Gpx@zrA+D1nYGx=WY`iwU+37XDpnWj^i`DT8Z@Alui{lsnO&7aA2{%~VQ z`qj%fws!YeC--%vd#BTEpnT6T-n2TchH@5K^7P%o6XD!4bKJATzC1r>ewpuz-y`2# z40*??>ytDf1G2B{`ie1-`Bbqk?#cU`#}XWw{PZ``5xYbd~-bv zeQ_`O-q_LGy}2!Z!Z}6nvhtm0d{chSCG_MEPu(5Vh6>^KZ4RkLADD0Em-%k~Bl6Al z(0}lEa=hPmKG@P_--opJ^|Wocp`-Kqy>&b*-+9Jg6gF374x7_zE}=V>X8R6;!^|J^ zjjcSP=q$d(d^5ky_xOJ!-&_wB|C($2zRoQ1^*vo%J2$p=Y-zhb@9P^gi*LxZT6KYK zwKwmae9vs(C#P(_1LyCGyz9t6fca*AneV~>ANl5b=&#PTeVZ*BXJ)NkJss)JzP5bd zZ_g+`b3RygNgl06^RvU`8$+47{%3~+fJObE`DT8ZZ=Sz%JyhSDYx{OS>1^9F%}qO8 zo-Ci_8Gk`vdgiCzVGo*(Pfl>Tg-LR~*O|=~zI>gc-I3kJHDl-fnG!+`zR92?I-N8Gyh!Ly<^+jvv&S#|H)y! z3zvWV3zxrQS?wLSoE!2<-Os&$@y~Xz_`nmvN>f$sWhVYlCg%K4ef$eH{xDP5%_iQ) zSbmqO*Vc8Xi{JF=()%Bo`09Osy7E}lo{9g5@9nIe{Qk<%T{(P?Ex)DwTRW=$@{^q> zzU!aEkE8zjv4g(2{DWh)+x}-q$cwk%a{t!ly(|Br-v>%U$iD7uRr(9`>QYh%E}$NhUp8;Yh9eCxr*MGWn#lsU{J@3D-Jl5o7a+34r z#zF9>va@Q<-%jPi?o|V|cf9Fi4JSQQ*HF9X)CSGdl-(b%HAVTk@jsebHjvH2h=daom=UCxv7;vlm zh;WvTHJ|eb9Q&X12PW6AFJBt|W!Gh2vGsk^tjJ&4mFW#O5^LC-G0VvOF<;Ez_)6rD zc@3Mj*Xaj(cdHAn8UBfRagUqvjfCZ~vwl4Fe7g^NpY!iA`T0qhBNrMQ{eFu2Y-IS2 zi&(_$1M|mxF@G&}$RG0>wp{gXx4kvPVLtDrd+%~@?9T|x*}9rcJsf9ke`O1T+vlGd z{$*Ee*OlG=K4h|#w7zVly`qq`FK5>_^H-QZ=8O4bjm#@}V`H!@t?kd|Z{WI+2TX2m zH-F~)A;AaT_Q!njb0W*p`D_DYbgs<{GlSfQZ zd)~nNN_c;a-^{v8&o>vetJZW~-?1Th;pwO2T+}Z$HAl}+SaS4__#Vvg!5CLA!g%bt z7`I=D^OV6`lFwHvAB(@gbf?O!KJT$Tuc|Up?AN1a+V*?!DSX~yCKCt7u^i(jTpa6D z@H}&>iSwu`GiEu)H7~&Ovp!qa4mX#_<5-SyBQB2hAxnF+UOo#h}+fP}Z+o`|Z*}U5LL06vf6OLIgw;$HSdKu61 zjBj7;_77JNx1(I`vpnO+Ts-S#`>e-rpXC|fd}CWxKiJS)TC~hoXJf%l`YGWBV-6_dFuQ?p;vtI7+S^KZse^{RJ z!!Dlna`|(`vpnM~pM&;UFZ<1Y_}f3rGk(y;vtF+MERS&icjXyB;h6QZ-CXf3&-iwC ze!zZk|D)gh{&wAPKr1#(=8ySe{#ZNn8uFektISo|U7QyLzjOHu>blKJeZ5<;;fawc)aCdP7eMBG z)nlh{ebatwVm<&WHDhyq`1R!_LAUwKJ=kpvXPWUc_OmApQ$@#PW`*)`_Oh|9uK<~${h`Z=z?1U$4hzx{@BT<^*8 zcQ&GBx8DSx@mT#zn!4I`KiqEy{@KKu{f6Vj{pOm~(!l;@56n-Gsnh1i9}uOr@6Vqt z=6=I`F@LO|c?~&S))l)sZ*tow8z(169y^`tH`e_lz0%%7dZ_M)Y1WR#~nU2!S4yYUUv-~R0VPK@LCsfO209p7--`t_Gw zyw2W$x}mGHFWuia-5#P`q4A9~X*AZwA5N}Goe+CvUx3|SZN8ab=DYba|zPTR!s+2GD zrTTPFuRnipTxk578MPYiRhP*(*c_U}p}0q2w84Bczs&bw3-ZnN;JTCu5C;A=JduYcOOYZey!V%9cS{*`$y1Gem7u>ZWla@`DT8Z@Aj*a zZ>|TwA?2&Ly=`M&n|4@Lq4Dcx)M|7o7KHh>uW-9A^s|a+?YX9zZ|0Zzu4qNRxgPv@ z<;&b)ep`3r)d=#;=`GtI-bq@oE2f8NEIdEjgRh%s2DPd=FoXd~-ecZAS9d zWu7-+?rt^TipfLag~qQntyZngx7FxU{2(jek&hjt1!r}d`DT8Z@8;`}Z>|TwQRS<* zU&y;m+Ye%*{(jdtt7FyDunm*|syd4Cl1%Y5_wDc3`^r*>NT>e|}Zy*1Bwmxae) zGNV?bOHpm|eM|HOa~-ny1GjJHm-%MPTn}W9mGA4?ZpcU6u7X128)nohdvC{!vwU)3 zbas)ZZN4W=-Ms#HkC~p%`&#aUMe5e9SBs}Ls@loJ^}IH{>b~b`Uf^|CUWcvegkO1m zmY>`4b0A)i<@MQUoyWKnAA6ngkVEVz_t_a9-h%o$9=YDNw;AJTeM!@HJ=*)+7{}{~ z)g4Lk7kbxScQ>IK)h>#$_bce6Q4xIf^BL63QFp6iP0 zf0#&{+obE*Hqst3OIiCg$3w5gcEj=B!su<5_r0Afe$6H4U(jeT3+DNJ6V_K~yg3ET z=dmhmHZtBl`Sr|w#F3YaJjwl?+b6eIHp}f8yxr~J-HpN8V;%~g_ymy*T{+CJf0SBxsxw<5~#}z%OpX2H-w;y+7%=&VTxm_`i z+tpBF{4b67?BI?Y1B={t<`^?`{=dRq&<&3xbvD)tMAj;t(M8rZ+=A)230W~aqP z?YrX$^TqtJ$INTU17AS?27VBZ-!PF1^LP2uQkO?YC6AfEhJ*{_3%-B+dAvu`-3Vzu z2VYszSYrRO>tp^t<+d-sL~@j0s&EeX8`D2-zayr~gw>hVP(JSk7rJO$Z`m~K&uVb) zg}DqwGC<$c@AN&t$Iat_|Gg=CpT_}Kxyo#uwZ{4MeJb@-X_D{92Ha(>@+bDY`v9OEa=amx<){)UaU-`{ZKMBhJEx-_tV*+VoxNxnza zzVkiv#r%!-BY(^*mcTwQeD!WE&7#K1ZD2mH)}Cjb;_{UAyxQn|)e}zPeS?{F-`_A# zZQ353T6$Zs)YTU)Z}VsWa``?L*rSAfez>({@dlo6Fkj4H&1;Z9<`q4#&-eZ2gY3<( z!sq>hqxs{53v=a<^I4A@`}qr>zus@2_q)j}Xp!5W&7b}A2YhqLKJPc-VCxqyfSEt$ zi}~aJ&%C0E7n%HRiEMx7`LmV>ZjU{G_Fb;ace#G?oR`e`UbhpzKXh+!d&%AA&ns!r zy3PLO-u`S-?ek|7lAB%Kj9+2?m@np!6*8~nXUjOc{n_^e{_s?3(pjEgYUW(|I_#e| zfA;x6WIi8g$MHwUzbs>#dTI6BvtG;fe4rpa9{Byk_hRF(=6WsH>+$GZ?aZM6k%|Xu z-*eWt>LV|NlifGnkoDT+c7p$_rBAWS^giLvo^LZ z92PwFl-e_5_&Xo2T6uPD&6gkF@!PN8xuf(qPwapN?KW|51OAc|WcSBwO<|r7^7%l? zrS+<R^Fq?(H}W&3OYtorP+wxVLdOzpep1DVERgU$PP^9s-RdH&Brd|$xN3wZwn z@1NlN0+zSqMzerC|1T|VN+q5Dx4hY%C%JL|8{B#48!?X7mo&}eALDrZ8&8TK_){b5 z;PsumJWgssui53X1J3_HW@7F6zZ)l>|2LWMZri`)f%!=~|3}omJO5|Cm_PQIc@25s zoo1Q;^dRR21MfHQ2Ns6sW6Mo$n#`X)zvuOwGhM!T{$)8jKT{7XH*OfHU1F}S+TUL? z`+_-tZAvwkEKi-0aQ{Z*QNa6A5jX~r$~lnt5%+%Jdb3-J4Q zGwHm|i_OSW1hTm{cPUv|2C|T z{&zVqyK&_Qz=Li)d^~=)G1@0P9nHICvwIgb*zKpv)G+G$^{AP)>$m3Z=#QC992m!P zj2m`wtWUx5`}SFmah2~td#sOf{{8+e$GAZk$NCh9DfW})7&qbKSRdoeCqSV}7{_vq zYk#L(PcEKuHY`V@yL_LJopH{#-0ALGoOnBlh# z7{_vqtG*5Gu|CG_nHP^^ImQjSIM&CwiIR96%Q3FPZU3x~aeLX4kJhxMSyR-eZ@hs2yQ5Vm88PDyK z^)jC28DH~mw9k6kuD?Aop5+-o?BZE3+szfv@{F&%1MRb3#`)VN+h=*k54w2PTj};6 zfBR%S%QJq$#j{@e#otaD&+?3KzZ3muydDC zohzQ@89(CUSugwT?>B6p0|QmS_Bsi)X#;zrX*neU@i@#rx1c>t+6O z#j`x)`&~Ti<@V?A$84YF89(mgSueN0T=6W=_?98`pY^iex#C%#@uM!D^>X{8pR)FU zSDx`T???Nrm-)*T&+?2PcJb^7*L$vbmS;TAPgpOvKW;~U|5={#oX>i>|8hIxe7pX( z8{l~m-#52>1k2BI%pX0P%ZIkS{hq}CF-LViFW~CX=<(+Tw7cKU=D7cZXm9*O@pjq1 zKMuD$bD&@x-}fcPKMS81@EeyOf4TU)0DBB9=D_?hU(6rdWL`rKpN(Lrj%on^wkaG3 zZ*J%`>w25}<_%_MKQCa5Hy^S5-(FK2s4X#PA9`NEw(`cH)BGh5+`jBHDg41k?u&u@ z4fhl7H^T?ueFXO>?r*-o=tK7P59X39pBM0JlZ&#lTT{PD_ohMz-g- z-!P8rJvrWgUcgsfa(VrUy-yz7YiD-A^`?I_vG#hC8z)|Gy4753vVYkF^Yd+!8=D`0 zK$QQ^*DqB#bH8D}m_OFcyoMYutDYC|%(fkk&un{9c)jU$CO5ab{5sip6+z@UD)nM=O@2^_Up0vv-htvj^9%q zy?g5Ix6u3j3yq(R=LNhrb%LLcqBHZ&{4(F|pFqC39{i5zevJM1c>%Be8$K_9`DMQU zAA9EkXJv8y{kwE~SM1B$u!16r1$HSa78JpPB`OLlSO6)CebpplVyua>#)6#)Dk>T^ z!mcF=>KZjgQKP}cB$`B{$zOvdKJWR?%x|B$5BFYnL5RHG;WNyc-<&yf=FFM$%zf_N zPIrfSQpC?wd#|{^7ohd3U2vu)T2Qi%xR|{BTlST`r%(6x);MIvr1oBMe=p#?|KaZi z$iA}ozPiY(cJNS9F(a7E_?C%9+?Y%sIFF^K{y}P#e_SQHQxA%(sdjZ+? zyEWet$=ai(dHJ{OD|_qtRO679m-_dL`+EWB{SSXHK=zfrWnYbh@=8;CueiS#kX=uf z=kEpR{_g=ZXt*YlX$z)0LlrFi8iK9;y#U?2>K?Y&27Z0j{cQI07k!?S{XFGUduaWq z9W+OqQSY|<<%){`w5cv|4?AT&FJI}@l|y@L`gCz7Wu4;p0#r};h@Dm~P=Ar{{q!E- z<+tn}_LJ}+?(bo#%ROvJ3*N*2$G+o9Tr|Y?xwwGi`C7~Gy+7GY_M5z#x1a3ld+_!7 z-VgR8UK0E(YvGFAR#v+FzV{=0$$syy?(HYLdi}mW-}{mOVLxIv_)h?q-}ioGFWGNc znE$e?*U!G^Q+LHPsjhtQXCs%t-N4xv*S2_en5VY|Mtu%IJ}wW%O8n;lmP@~PHamBY zeeY*fkS9h`);3~s|HG)u?|VP8m+U89Wmo0DE@|w4><`)SpEk_jWf$hH<>x2+2jBZq zUf=t%zh{F#-f2)=5etOCm`#sfmw+#o}eB`B1zwq*1{jdLW zzkW;G|E1M`rz3VWJLHF!V%RVHy%oOm6MygL)IV*ySHAQl(K4zuzr38ouA6mPwcim_ zFY7mP;|ux~vIz6>1T&z{gWtsO{rKO!_{{dUVqxk3E_hZlcAsXT&igw558TwRlX@=D z`%l^TsPukDESLR}aI%f(+H31gT#xDh$s2q5O1pOU`KcTFRD8u!o&Qx&`(MHOb<=`P zb>5$AXLy40K3D92^~at67tFD`cK%mtzKwrVm+!5#%klS4K5V~{i-jfT$;+@%lat6Y z-2|-#*-!S8{iKfU>hi9yP(8dI7yK;P@9zG8u(AFyjQ5}-3*cKN%L~1~^kMshEB;_n z+T^F$4=dqc*+9)Kwk-0_ekQn6@Z$!f3pFgP*Ve0bns8yN3H94qHpazE*%ZrYZZsB= zol5f*XWT-8@75Fw(p_nvK7A?+@qJUUac55)HFiw#-tsrciHy%QW78U`HB;Y#(ORl^ z)^t}=>>XgeTlBt7dd|dc+g?%evjX1Kxr<*LmG;`q=cjMpx;0bvH0KM}U*x-z|ND2> zuuliGPd4b?wdJ*D%3iXc^pRa%ev$7=&b763ji)KoyKBtXX#f^<7piAg@^QK6(*J(V z%>S5ta=Au-yzVQw)jc^ zV~dFHu2))VVZpoW$=i6lE1kNf&v)9ob?dO|X&otCKl2-fxjqBDzAZbu(jS65yEcY9 zYo!Ixt_$tnnpil@;3@AgHo%8zi!Do zW=E#xb^e8>rrv)w@ZT2CvB*BBJbc;*gP3wswm`E5^xb%U7hd0u?>s)N$CvqcD4Qz; zV(5^=kEq;d#Ax^B_-tVd^X8_Dgn>Kwc87KI>AaA3-PPx(hV;=Bd_F5+S$#KNdcAB7 zqW7ag&ush3vmR7hMtrK*bw{ta8g%vE@g?S`f=1<7wJAI4$+u2jj2;-}?r_V3kEx%pl6$)m&rSpRGIU0Ce% zb@yGAsb{^BWUxqz}-`4EVK|532YrDU!_VYvOuswW! ze(%=J8`aae&)c(5{a5?@*7e*vzE#h$>W_bp<#(LzGnmGZ7JQDKXTQ5lEFALWWmtTz z<@X#bd&z#XzwGMOJEniGal$iF^~>ad{$D0O|Ga_vn=LPfao68y(Y#S!<&qOpKEJA2 z+3Kc$JNx&!m)bsWF~b?M`kO6LJoOc+T3Dc=z4rEgqqHbvC}7>@yRhQVzB^m2 z{{Dmd?zazrv*qM*BgdRH`m|zv#qg!Azu)+g!zYZa9K1x0JEf*3&*cUeQ*W8!+9&0o z@>ew|Klb^${z`lF*uLS0`rlRe=y8dq6*b~7mHJ;rvQKDR>qG^FGB=p-tObSq^&f4% zidG6VoPnJEx}@vwBUWB&UsCl_-y<3!CQGyL{6<9)`C(r&H`Ld=_m8L#9m|QB%VgOC zWeb!oP_{tX0%Z%7El{>V*#cz?lr2!UK-mIi3w%Q@z@0_;zTg`=_~mYwEl{>V*#cz? zlr2!UK-mIi3zRKTwm{heWeb!o&O6Qkar)BA9G zKThwd>b+I*KWp!V=x;{wUPwj7BjF8harkup*zjIlNT(0?`RS(=de1GtE;+q7qk4L8 zrt?V!>SuqetnTyyUZUQcTU36GJRIJaaqr31P}km*Q(Evnxkv0BIpU%rwvUsSVez$= z-+OYhm+aTIHjG0U53gQj|6J~fXHr$|x64AF@5W9G1xE%uT@mtnPfmH2hhilK-jkb_ z8`kzpi(*Mr>^G`iE_aPV_*XVi)Bjdkwxo1^$_U?YrKP#O|M8xj_*?p(+!6clb7YSJ zyeKzu?AX&L96$ENQ%0UN@$|7HMlP;JU%s^U4{_~P9!BFWYcb39-hZ#mHUB(Mws4u{ zHQ(i*^4DIY{kX}Gy#ihO=W_pgO3M7TcFq3B9PRV5zn&Es!zYd%zqsa_ z%Ts?{vigU#w!!9>Ma)mG%JGfw7z7Z!5sWBaD&OY|XeXjq- z4%LmXRN8L=cAfq78c)pq_JKY7%s-#s7HoWI!k`gPu66ITH4p!AVdL}vxux;F1CH$b zpPlA54m;&iu6p8Ay-sKP_9RdBK5Um?PkgF3InE`}>+Fzcw0!bw1noPyF(mtRG+>>Wg3Xr-b^FtNPh-RekZR{=!gSa@B5j zd{tlksz2%+ufOD~-D35{uln;teaTh5te>cT@vHv8bA9`gtNEJs7u6TP>OUIlORn_K z`i<&~U-i45=i8TDwVU-H)fd0&PYv}YSM{=fr267l{Z8lm_9a((7ppIR)qi(}|6LTx zmEGmv@=LWZe$}5k%dhW}t97VYeetV)rw4p}$(8+z)fd0&PY(4ZSK}`|vigf(_1_Kk zC0G6@J7(*PU-gHD{Z4Xazhd?mzv^qhlwA3r{2<%D_*Gx!lB@O$)t_d}+Lv`d-sz|L z`?K2DI#O&spxqDc8Sxq04cXZ`cKoQ_#^D=__SoGVru^2+SK4*9&rf|gzuo2v8N&IU zEtIOK=jVd;>wf4LTm5!#<<0r|jO=Wd{t*0qHg)ZOJg3zEegNg^Z~1=IZlfHraFBi8 zbSH*<9$#zu-H*#&vY)h(U0wcb`1=d^mOnX3W1psfjkrQt4%C18wBR{jt-P{^$d}JpCcXXntIn00D%U8NE%+pbiwQk<1p2odk{mRSp z=gHS#-e8DApDQizc_Vwtei|Uz)qWbG(Qe8ZN(%$~jhnMp<0t!`-<(&R=lYbAJTcCd3Mg!2R`8V;u2I~X%InL>n|KIXn zA7n4tZ(f79pX}I#=o-R_pTP7i!9rqbDiuZ`^kT0SNr9sXDkk|6Q-pdEzRbaP|v!( z_RU$G`SWdd&kcThKQE=!uW6+1tlQ4pu8{rwxum15bp?!@idbq&_LIG2KM9syef}J6 z1$_(U@iX=x{EL+S5rmUhc`jFpjB39><@)*Gxt-TG@F4$Jysx zI@isP^}(21o^+}$^QrbqCowcqXziu zFZI1)e&5~g6X@oAMr^?>{UNyXXyYnYm){o)X~FlVy4!DL6AK5~=gjP27`v~v{GJPC zFWK+WUwZq=uD%BYZT%ScWZE0@H@W?6o`J7fWPLa=^yim>QTrG9xI7ex#6MTBY)dcC z)dsffU=k0in3vnLfPJVm-{M(ThJR%PHJzkv!B?D@|H)pmpZr&L{fe7iYH|1;v6%gq z_j?htm+Yqw%C5QGnos6(7n{FLYqVJ7%h}Js-CoqZPvL*~*%6i!w11J8|BYSyl0AuT z{~Fvs?ECh+{=N$T+wOE^1zuDr;OnLASG;B+X-kf2|I+$7%S07XIj@ldQzozc>ynxO ze1Ub`DQ7*1I)NR=87}@U%m~;a`}yKm-LI^z(PNr5qJ2~QsM^*3s{K;?r|z5eJShI1 zt&g13wU2h{)L3vI9rzn>C#6$f2A&i^fN4r^F_R)|Q+(#R24<#-dV*8w#9Smdlwc>rWkd#H`wic27WG~ro)Jxue zvTJMQE^B^#i}-cf2V^hVZ+`G!+10dt+M@a2rIudW|AQ`9K4Z~*NY90fx0w7-=M-0? z_;a9Hr?JgJ6~skl%m0em&)4d>XvMD>k^jkFvY(ofU46b~&w<)UpAGX>`zU$sqhaqY z^&FV%lzXnA?W9|&G<|+9VEj@QTUmzvcn;M1`JmA)X>mn=4h%!vY^|X-v*xdo?wdbp zp&f?yA9~WrF(Y#mho3w$H+*!7zmV75)FMP|?_O^g+IMVxKFG&A{n6*M=_0~g2%v*v zp(>L@d++|z|J|;&+H}qxeRS4}g=&7aSfhy>U@zUJE%W@e_@KUhmSG)Xd7|&Vla-NI z*JeS*-nq)aft(JxU2;Fj)phJ;H$9zlo!vS6XtUm-R<=Q|OS5iumbV*h<5<~IytnCz zVrgKrWY}N)_$p2HqppwWl9k!sT4(KO>&nvqx=lQNT(>k^rpgv5Ti_dHfz8|7t=~7O zzvZr$El|h;FEkxgt(fke;;hU2PDTg!eKWt;-rMGQ`z`p_n$HI9Y>%Dos%bCxGWs2~ z&%p!s8FKs(mq>1!a(1EJ+Ph}hiLCn=IXq^p*?wk|C`!6_oN!M zmP*e*am`}=ugJB}@y-Unn{RgR^TrzcSJ&3oVz-(b>Nm8srf+TSUF7@PeP&<(iDP4p z#^QyhO$^WFDqXC|)tS84jo#Z*WQR~AegR6K{&2o%?JC_LLxu?$ZI&bqlQ>b3c zJ`s%sFD2=oXHACA^*<=07rH7Q*QQ&W&L8>nBK6<30-e|I41Cky@#S6r?9=HXwc-%! zsyR0@H|(2={Z?|)>#Dgc-lazmD{GglbpL2oXHazTPpY7Vg%?fC^Uown>QvT-zsu3a z{zajqjcNN_lI+raMy7CJzx@2XOg>xQ`h2Q_pYzf&#j{jhHJ|n3+i3?rbG{DdeY(+X z_hH|GZ=Yjp4BwHFo$9Ll+lM>(e}lr?E!)cC^4vviVX^7wXETSYW12F?zn7&N@7s)~ zZ-OF4h;!+Giv#WQy>2~EU28P*vSG-`F(XEfKWO;bBgf~)XA74gpCtZ`uH-^r zS(<@4K6Fdk8Gq8?-ip86ZOgqCv$A|1n-G-d?0n+S)45!wJ4^d={Xeb7f5$`lywu7F z;{wBPXzZL4s zA^psvW-^`EITp0U4-V-!$Ua@@H+IC?J9Y2AOCiaN$E=-pi{omEE{l_~H052lI3Vww znw2$W|4rTZtA(R4clzA0`&)MPpu+Cyy>0W$4P1s{{wLmF9^G%S>jws3tkHu$J)iE> zM=@VT&82i+=a-YqmSyui=1jMO%t3YP+nY z#phG6bG)`gr7K=sHX?}XHT$|}Y1f`9-E*QnFWP+x&y3lCU3fOPz=u0-oD111Td%cw z-%hjq8fhKNiFsmt+0hHfbaC_j(Kr0(j@f%0v@E3Q;xu9%uQVpjSNK~ZhkDFs&F{G0 zpuT23d%zp`AHN1{9c0d5C4bGD^4!1kmV`llS=R>)AAc&+mMoZKv^h`@mjy&j+H!br z#pW??_9cyLTv0w2mo?MjoFV=!ulP!*{@rn0Sp@VW;B9#Wh8{HrhPxx5 zruRrzQZ)EJ7cKsp6?Fc*`AijTkyTLr$xk_NVh?}UCOe8JDvVI<(3jCZ|4bvt7G(NgU%c(XlQ?^uSLR z8|XJOVFoBV7FMq*ZnHy&4oNn~<)Vw} zAst9#Jxqs1=Be@2N?x|jVtMf^jrCZJEf$#<$WS{SJ9bP;Me!+B`%0z9VsvO82l_5q zal4&5bxPtGmx)$AQngQ7OowP3nr&dbO_CS4SBe%tX-jpGKS&2AqtUh0M~>QCX{D8t zVo^Lw3+YkH4jEr$jjL7L#o8={t9{be=#a5NJ?$eyZLGZV%1KM2_@zfnbdWzrHfXZ3 zuUGqVyQ{3SN)pFaS6ww(ZMD^0gr*GsVtU9YG#0TxMmBKmBSURx+oY}4S6@9@bImoA zwbx!dS!bPfT)M^@YdBrtZ;cKa8|1aGwrNxSr5)PbaKjCg%{JRCsjRF_Hr;g7q;u!a zP7iD%9kOGwI2)Mnv(MF{f1$SNH*IgS$tKCR+isim?AbHv)vH(1qeqWqn{Bp9Hr{yS zW*syZrF6)S0qtAtq4wi`)Am|xt>yYn+tt<8$5x0?UI=~jyz^`G{&?rgBZ2Clu$H{aaZ zob`aV;n5lp_dnad_GIg4^SI}??sKMZJ@R55Vm)Y+@y6%Xw)!5opS3^jhqep$J=P_w z1G>;w9Kq0&w$(mkACEt_C(^e0JkL&xZCiR2(m^&++lAW4?pfPg+isnc?AE08jdlvf z#r56#&GS?9Ij=SC#=NXg)~6=5rpEQ zl_-tt1pekbLCTu>GWRrDUh$Pq%lCbG1NM!%@0-3x^Zj?mkK})a{bp(J`_yi!`@T1F zzt44EvspiD}~s>?lA1*-&*s8uY7$&dD>gm+Ol^6o7TfutG9=>)5l_8i~T~Z3D{ni_Oggx=z^YGS;S7*M0(IZ5r1h(->vOh+O^+LOb)Pk zu*E|x4i5AnOTqWE2oL(~X>m7;*ae$NkIqIjF0@Z-HZ7?GZ6l9153_ib#bYcU8=`$5 zE*WO=2#Z53q8GaCYY`jmZE<&tj2HG`TsF3dEzkj zu@=W$oDd@P85YM_Jk26Hq08|Wu?zOX9{XGDZ?U&U#sojw+9Ljm4%onKvB*u79#Xb7JqD!a&)=UBDTR+*o-k_>`t+WUq}b+z*y{U zk+p*LiaCJ}tZ_CjwTvx&qutvr-fi(7i!(xmo^J6@i?>=tm#G%7v4}k`wffis-(WoO zU+I8f;t%-b9u}DsT`banTOaE!UyIF=NgMZBe9+><79R<5cEIkhFr6&kW$`w{um@v= zE%29%ES_VLIfD-PCF=)1xxYnx0UO|ptdVAeCd2EIL7!>&F^h97Hdt&7v7w@EGN(&myeV=9V35!o# z{H4WbEk2)Xn>=rAJa272*S=%&Y{!+7pLbe0X|VKB%inMK-EGzPF zO~wHm^tQ^ zLGoKmpSAoP!yjzdA(lZBk z&f@D9-?X+Dv}vEbV}1Smn(HJVZ(5oBciZig&v)#WeAIQj}tq;Q{Qx zSXjO5EaH>Q3+5(sLN++mXyyXzfxX}9{LlQL$4uk@g~i`meAD8e%+}7IH8i?ONXOVutWASg+{s-3n|8?k;Bx|jkB-Y<#Uu*lM+9Vl!Mv|O3 z!s+mThQDfK@wD0K5q#48fcc3(USW|nh55kR#C$x`B0jK}Mb-h%SFAy{cGlDWTNeLj z@gr;hV{89&+Fxh=B-wtaBss+TP21;Pm?Y!IC&}O;NwQP-WP$m^b2bJ~So;rH`}h+6 zj18C%tS!uikrt1#X#VEfw{^Ilwh5J{2loGi#rLi4Pb_|J?I&jc#Ku0^dRy!7z$6)N z{T^#=j~taG`yZGj+wG9NWBq^8`v0W0&w7C`VFUc&a%+n*m}n6nINBoZGyb**Hn)!s zw*ECS_W!iDKed<;&Hs{>R!x!(H%XFiyLcTK|A7Z2$*#L4UvAbVd1tjXlV98T|J?lV zajSp7#XBt4Tf`4plO|cDefmGpqWOOF-b332JGb@twekPm;%91KHdxhsu=B=dhpsjj zJ0{6C<_|U>J~sdVQ^%E)-PHc{qYEkH5Se97CC>QgRRrG zf3!N--}RqVI$(!2)=rYO%^z({K3`?^g1 zGWNFa*30MPHnX(Vsb?PK>EQJ6_W0Cn@-J)mJ;N4QWX+=gb1gn#kv$auyVTm`T*3Nh zbFRtu+k$q_wol&HuKK^2494OMi(gv&uSInF*y0BkDaU8=8|DxGcdtd}A7jrM3BR*7 zw!SrO$NC`quNFVF_Wos&lsQVgPCg-xuS|x7G&xT%iZiKI5uaw9h;E zT&@+N&KrY>E&xX*O?T$j)K^m!kj!z!XnOXMz&hj!OnZ@v00 zx7^ZwzRBmTihSO=)mB?2TWqmKf=-3>u+L@}Iae*NO_kecXtmbICO*U6amO8#op#zO z>DH~Ai_l<{@!3B*Nf+!QJCLGRYwcm5rPlAT!w$(VyX@jV)8+GAKFcOT^LaOAJMX-+ z(+6GnytixDuFg)_B-R5R>~qnQ=PdQxe5w}Nw8iK5d+oKCi@eVOz2}~LI-Jj$;gK%g zyLWds!Y1f}J*ujzT>ojmB|6YH@@S_|pFRnnq4(|EH!p(mt_7dBqZ2yrzWeUZF1+u6 z9vU0Q1>cE%)jp>z>YHZsCgk?(*Du*;pM8>j_uV(yZ@>LqgeHOy7%;%`pbL7U2X>Jj z*biS}3}p-XD7xU+vbRzj^IH0a{DA`pCI=jFKyu)L2POv{bWn3755E8Y`#YWB?ccw@ zvlDt?H|)W<#J0dk84ubfGLO}+QtH!J+Bo>&gOkC72fKGjc$Y;H?2tnaNd^rX?3wqQKa0iV!VXl~fMLCrQWdo>{o*+(9EWOCF|M17lK52j&Lr7QR5F|F*8yqCfIz_gmlkR&xCD$0sM8a6&SC_;44YiQtr@ z54`9~J#2>lj2*s%4va@^2mFC`OXEQMjDgvqmT{$T$UpJK6O$1mMkFIgj!f(~Rq`Sj zW#|Pjwm}cZ1zX@7jE8iP9qDPj~M$@qQ09W$>MJ(n-m2#~tT%$4?l)LOL*~nHTs1^Aa222ln1rljZB_ z2W`_X?a>bJQk`+e8Og+n6O*&fI?KIRb^iJ1yZ4LEKKtxs!h{Kq7aK7)j7hA6#)7qi zb%MExKVk#?fcc0I(ms73HENXW8*L(c(xgcV?Ob@_g~_FtUYcBa<(0_~fB3`XM?d;e za@AE=IgEF`(385c4vYo&X3w6T+;PVp$<<%$ZIH?0{d;59SARlzD+)GY-s4Y`{9LF<`%>eS1%+2fFjl82zU0i!Z)7`TqC6 zpWJxkjmcw=J(j%n)?4-yGx_0@Pd-VSnwpY3@4PdaGG&VMlS1Pl8?ZJpAK5qAA821| zAijs6(T}>iy5zgx{jTfR-FM&Z`uW*spXE0ma)0^DUy`5x^ry)+*IbjFbIv(#3^ga1 zm)L;$fFCdhtk3uW?K20o583>vr+wL-{@`mfX3R)_^PArkY#R_8G&D3M*I$3V^8>~( zJ0I`^#sELW2UrVe-`4!*^N5{IdSHLr|Ms`PoqX?m-*fFxpFTZ#@x>R5w*T*c|2vsG zcdl!nd4m74F0ei^r&(8+3#>_MpZ+rk8GG$Nq~^0tygT@v?|jF#k43|JbXYhOakMG}b!wt^I=h+Y6 zWOZQdpMLu3X26V*QB&-}A@r4~7Vpogu^wb+sI|G^J_kX(E1 zwQlY+&lu;IUV16{?QehU{QhS@`&n|=U3a;@GhdiHtVOKNar?4A>mX~Ot@n9$SO1lo zo$8S%8=wPrU>*3$Pk!R;fnTsDF-FW4Y=7pNXSy{+_GcYn{P925Abiixqr8_@ul}JqcinX^GB+3>?8JO!?lFJZH&_GML-9Y>LDpgVZhLQ&y@R&cwxtJl zKsIf#7F~Y%<;{_IZ0R?4qVL$A`NJBnb(pbd{lo8Uuc>cM+tPs)pS9?IbnmAV! z^YbOqL3M21t~L4f=w$0_llfPZtuIYBuj=hgQETtRws?$@srx?N?`>^Zd@rDTz!LX; zyGXazrnGZViTl1wAKai?QFmgCrg5v>xe#>sl1FFmX|lYkQ#$>s@B8dC-wx8ZeBb8= z?`!$ryk7sC`#!Z>>b`HI+{U?Hdo=6k?5F#_7?0<5OO4EX|3urj+&jg3kJ#SX=DS&~ zT5DRtvFydsCZpb- z#oT=v&xN|A0uj*}Tv_CwQUz zMeM?NPo#%E&oqC(0Uhw6QaaE!{i98u33xu|o`7dGQr&g(%*vUa=SFnl*@b$#PeKpI z24BH<^v*x2&6lEMVRs-+$fZ5braT{VSD;8R4$m*K9=dOpE$|I|R5}D(EHW?X2mPcCo{u>r^PHy0 zGb53^9L|vFrTZLo#va-GJ$#pYrC0~r#}?*~&EvqG7yUsl?QurttjbxP=LI4(7-c-a zp$mGV2lhaBY=NK97kmgGl@5#r{-C*GdNkXBvpBMlOc0=o>h2W?ZlP5*6uVx6nS=CsXoGHvk8$Qhnz2+s4m|KctR8l1a8-7R1yYwH%<~3()S)i-eOVnC3+^|V7x*J< zD)WIg(&jsBfoq?>({K8vcDYC6xr%2!o;P{coH})?yW8M7m*-*h;+co%JUwUW`5u43 zCz%)WM{K~l!1~BspnY3E>#0k>X`5#$p5=JXq+Oo1x!b$<-h17h-+lMpm*{z$a_oT) zj0e8MTwy%%N#+G}6JL-GSf}~yhjr1;3+}#``#x;RbF^HzrZ({H~2F134dfx$Ohc|vsW+%d`}bqGe2f8(A9KjW^EJDrzbemVK;U;moKcaC3t@rA>F^{ZdG zarp6%f9&p`@dMTg)&}MjYc^{OYXW;G`xN^%?VH`3&jQ?aVSnbA?zyZ-4un)9sN*9!YMx=_dC*W9DRbE-(h%b>ahRpYdnx<2~5y)r2q8etmtt z>pyp?-08mk_S*?}i?mDp_~VZg{D3m<`|uU!3+?OPnYEGifIT8^AN!l_oA(~+VCz8- z?lQR7;r^8SE$-Oyb!_m*KmO6(cmLxb|49D)=RYT}yz+`G<1P+mX~O ztvh*kSO1lozt*$1vEDKNS!*76-~qP=aJS37BcD5fu`a;}KkFs;POL*(k68=xKlWF~ z-p+*leni2(t9??dQ_s4~yuuF5MfAYd%r|KI%{<{gka@%WXa3>;S_gICM&Hfnn~K>z zZdl^Jb$M7>E{>hyqbCvO8-D2*se`x;Wd-NT@w>_^#_nc}wPVq_Z7nv8V z_lz}t#^%gd+Q#p>Yi2K`@2owX<81G5;%pboTN2vV@OoQQnrMqPk_gRSrGB%2a$cg} zw$?S-9?S3JEOxA0s}J{m-1VW8o!8ubpY^rL`dV-6N-g_IYh(`ECzorqmrnH?qdQyW zmfQ;r+Nb}d6Gtxj@3Nv}OWMiyW$MBC_Zr#qY<}2)we$W)h{}g`@ObCV3YUwoSlW5Y zmd(G(s(s;qkzzypzE;le4GV_O$M5TYlQq^2#yGz`Xz96XK`rg~EDrJGn$=}_>nc5q z*;DFg^<3V0>9M$UdFz#JZ*ltVU$d(uv(i{|%atHk_T)2eank8jqmuA(3&=$Gwf!=W(;n|8j!cw(KDR90sDHsGXsXafH_^R zZn($kLT%}Mm0{={u!eW8-!{m5;xWnlbHHW{ZpIp}I5L63>S!UCyWVVY|HV5rfQb*x^lP9E*-w1fUgJ@3 z@~BH2k|&ID(-?uhXY$ZbV}!hGPweXI%J%RH(-=t?#zVG;5B>Ix#z^`x9FBNfRH~f973=+24n~ zrI~NeuN~jzFT!`Tmp5mSlehLvmC|tdebgiqw_9aKf@+={~+*DZ?Kd1!BFHijcQ+G?OK0l^|*1U?=RTv z#<#1rbK_9oU$En>-Y-m_8?7e&0Nd=N_j_2ohWY-2S-W$v%Z+9!@lkJ>ljr*jpV`^^ zJL{nvhx-14%{RVo*3OMXeSg7DuzK@MpBsmM|3&%%R(afO51}{hqBU4|tB2m`CqC+p zbn<+EvCE^c@AaUW_d(4C>Kex9_q%9rv3{`*Exi7ohEF?QK6dVtx!gosTZmxo4{d+w z$G@<@s9`+tf?YoL(~jc%lg)Fm2M=jzJjz;whjjyNPRIA}nKdn!gGYR)JbTKs@7fbO zJd*eRJ##vWPwh4|K6nW7W*=GK=O5SK+NZYPkLUJZxX$5&kGH*l$6NaBV-f7S-LBiM zvpoUA125P*hxcCyA3R%Le67R!Rv$d;?m=GOc^h2qu#O8SUe(XOEwqL4i0||JKEH33 zJ?w9C8IR;mysG0u_~22y2hZwTjXe6GewbZz*qVB(3x=)#Zu&iZWc^&P-<*!<_e-;> zt%(iZrX3ez(+OsGTb~{F;30jhu>t+v>f&o%-M-Zi9zwe_tlg_m?CSXDbX@0f?Dvqh zd;SJjJHCz!FCUA}e>B@m-YL&sw;Q$({dIhw-`D<7`g^5~$Gt(9_wVVy5P9dC4W#qL zt3K^0dA1+9b{iTGo`rtM+jqxXoVkXPdiS$ioT)W``6#<`1&t= z{~l~We7iZx1$iG>J;}qzsf#Xe zhw;USu-|?rt165yevkd=#}#3G@q5MtUCs&P%UoqV*moq4xxo11E0V|jWqfVlZng{a z19=yiEa}IbmOR;tIfH&%n>_WGd5(VXS>JwYV}d{9FZAu_W`hT9Oc-Q z(_S^b&V6d`N;{s^r{>J|FW#Tn+4Eo9)r`2eM?-tVE;{ZrST5J54(#c%&sU4D{fnM&!S1(Jj|^>G zc&Be;=)K>q-aF{hc$wEF*Y=$1vnnr`y@ttaV|_s05aZkN+^71iYgprDpLYj)%j6w@ zZ;$E?4ZHBpi;f0sw00*ybxQRnhP8jO=1j0FOux-Gx}bVf!*aPVF9f^S`f*O+`@Fm7 zyT#kU$-C%iFYnK0tGzu(2PFT&_3! zZou9J4UlwegK`XP+%hp66@q z@!koXU~)M-dpLQVBPMp|G?2@!V;Fq4ryItJD3?3%wx2ltID^=!$DJguezK=)*JJ)P zbu4EO(;0b}zv$(iZy54C=H=P>VUC@}8tjZTXYHB3AOA26KF%*|&-6MEwf5jMRUg{m z7H@;o4GaA+%=hE>L0*mNjy%p>Uj})cSC9vGN~&M)@0)&{UuFdTIKzDZMNXc%+`l$j zYc~DhtVe%2`%InPr2ZP_PpzA!_Un!PsQc#`-hOt*GR(_+CXAc)w>RU4F1Lj-8s#vr z-%jQ$<};1>spiMW-@Sac&BKOx4sJR9hKJ0L8rO6_>F3O0H{3Vd=3K)Q(R@43{M-Dp zv9s}MUQRN+I*ksTcozw#~x^ub-bYeQloFd~N7tZD?+dviXd>c7|yVUS#XTQ`TkE8p7n^k7 zf3=>m?a^bwkj;9=cBaj7W}CPlwnj8wYBoea^%r^DnXSRJmSE%W1U{`Lwl+1K8sup$ z!FKxw{WKofc+h9p9FfRt@GLZPaXZ;;zi!=-H+5w^MfR2^KxSHC$lw zy}U_!o@xBVFxG<+Gd;!{^cTyszA($fGhO4B!>;Ph`ob&+gYR2z&B)pFR`YXSd+f)0kNuvoJo^me#y-P%Tw$2?*Z5NG&ePZF$zG!8 zJnHWI#E!dy={fI*Kf7R8!>hr>xh=W|OzZfACLfG;wFm5Y<8eK$?HX4#NE~H+VCbU# z<5lw?&)3*Pd&R2gZ}Q^3=X~oceDqg)RO5?hI{mO8Ydf}@_(FfzuE+eI_ie^M$kX1q z&b@BEub~a?p&!`T+89?ezS@6#+1S!9&=14>p7&vphyNiD zUppYkGaEGTDc7X>HSaxrgMRq)q_9W*^74B1w|Or+_NV8mzlM45{lsh)`rG^Rus0qO z?8kh=e$2O7Vce|0&3o+;VT_o++LPC{^?@@HYm?T*zZ`s6Pg@tO<282Xr=xmuPKx)B z>xPZ&Y3oyUyzUOMHHdQ;>zekDQ*BL~W#=>2Fslvf@vMU-1uf%KRlm5JDzFsIBR=(e*Zn*+T~2{+x7eJ9T(5+v&U0w zRNFnk+>TqncntS_GYs49sWlorw(}hUn`?Xy`}DOG>mBNT`kJr%P%5tv^0-q(p7BxF zu$}J+nB?_Ao-k_{T@3SecengKRv%qnJf`E;=wevI!vWj*4v(S7-5%=$AM)S}b^E{< zFyzhk@;-eHd3zb(T>;zq4v#sTc+BK|`WkiN3w2GN$C^K<37>CHuJE7JwDpV7GW&l? zKQCTp3Y7&Hv;h9Ga4Y*9FMPhwX2<7@1alP&RW9`(J65^BT@01jJDs{0xc#nc&VKL7(Ugm(oHBS;w*UEF2Q=lPDW?p4Rr`On zKvOQ7a?0Sb-!06o5opRqQ%)H?1mV~t#@BSEm9D>{;j!O`wNUA7B^rB(hyDtukHYDr zaQY>jehH^9!s&}}^cRl)!qHtgx(i?1=yfcjC#nA&zsmkHqUg%pdPdvtnp&uIsePPh zp~~T&u5|qN`@j~w98Xu}DBsZGp5N1+-+rsnLS+tKPggl!`#n<&p5N1+-+ph_LZ##N zbd`N`CzspQ;hx{qo_{lkSLWdLbd}@XJmUAX=dTR>@Orw6-|@?Jak!W7Y0vMyqSEnt zy2|lx5%GJv(%Dr!*it;$+uOFv*sn2Aov< z@URx4GpYFD*%sJvQt`v%4)2DOiXWcsEW%GJet32RHk?%a@N5rkIH~yI*&Wz$Qt`vH z1F+$w;)kafu;HZQhi6A%!%4*tk6pJ~sB)?J;n^wRq~eEn&w!JPAD(UjClx3W@d5EL?(zqCC_^{KSL4puX-pXxjTbi8SYS`t8C%JQ*g^K8kLo-6t6%6Pefk)E zutj_oy4ulIE)~A7VTU-pG6%h1;2{-Gy_$fNsxCYO15PUb8l(FLoK*bq>>qGa@x#+E z;H2V*=YW8diXWc-0VfqdJO>7xRQ&J^2so+u;W;Saq~eEXpMaB!AD%%0Clx=o6+bo< z4|bD1;h_wFsImWLH~KD{&@c5Doz)lgk)E_8`Gbu<)FLuThXk5*XrSQ{eVEaQ2OLZ^ z{FEW*2>buYKvOQ7a?0QtX8#`*Xv#%XP8mE$+yBP|nsU*UQwGnm_W!p6O}S{wDTC)Y z`~UbrQ!biv%HW}o!m&e)pI~|XpZ=ZzX*4{;15MvW)87*UPJe~dN8$8QIQq@sA2T$nkWQ zD<2*4d)o7#68PcubXAV>QzL#)d;Ze`KfIokH^@(B^Yrz@Q;#e*%ygRNvw?CtGbfV*CG+KvOQd&S=Wup^w6`LyUhL z7W04lD;l2f02@xIDHY0{=(}b9NmSZyZA3PdWuE* zNcz1%U)D@lI_&!aC%rt-q(2BW=@o${y)w|GR|T5%>OhlT6KK+F15NtFK$HF`(4^M| zn)LcWf47;g%%QW#J?;4`bMVBpm+5(8+PCZZW7_NH`D5Da@A+fe_tEpmw6}xjk7;i! z&mYs?o}NFZz0Ey;O#5;1{4wpv&hv}LCVs3vzxRPk=L6Ugzr{CVI<`4w@JzM;>jO=C zgQKe)?&-?hj}5yq@Q~gVc)&ef>FV7aaAbPg*S*Ezl}?7It6cfCh~Lwm|JJ|{ucxbW zl;7rXubZbm|LqR1biAIfa=dp${GRswcLsiVJzbTf{H}=K)1LqCzz?sds~qq2h~Lwm z|0jVTUQbu$D8DD-_q6Aq5%}Ttbd}?s8S#7C^WPiz;q`P?j`I5=eouS;`vX6`p00Ad zvm$;^d;SLkKfIouH8-92qx_5jL0ovA1lCtz_CHix+(|#aKOPtlRgq~csyO@mR{;mCP3 z@PK*R%Mp&8#{v(Sr@b8E$ay^QfO*=>5ssXn1s*U@dpW|9^F-hQ^R$;E9656W516OD z9O1~B8+gDx?d1qZPD9`U^R$;E9661F2h7u6j&S5WX_0YkwEsQry%rksMEZIWhg~AP3CTUXF0m-$Zge?d8PuZ-X2#PkTASNq-l~@wAsC z8s8AlD@MN>_(|snn)J0m!xPi52OR8;K!e5f?*k6@W}v}h`mKP2{UOj`F})z*U~dN+ zET;b$aIilG8Z4&Y2{_oF0}U3_e+f9)Ujq#m)9(fx?7cvP#q{3-4)*szgT?ee0uJ_m zput2l7UE$ngk$p$EV4d;i-!N7z=ktkqOret7$f1>TsY$*96JkVEQDiQ;R_-hdkV+q z;>U)TGc@DGjt$Rc(i{g%>CQw|8sby zOFs@g;GV8@yq^S|x}NrR|Ko69*V9$5d||}zY0v*@;D^`KRXNH(i}*e5`9BZ*@Orw+ z@qQ8Ud)o8=H}J#j>8c#%Uq<|%_WViUhu70pp0|PmZ>yZ6J%2@@;q`Qt`%te<#P4a( z-!|~W>uLME&**j$zo$Ka`@j#cr>h)qhltOd zz9D|bJjUe@_>1=wKfW4k#!lnNxM_^Ax$KXFNQ8$J13#j&NkI5qQ8nUFBp5N6wmo2h7u6j&S6x6?nir z?d1qZ&f0+o%+p?uaOA8Lc)&dEuje3_M_-_Hu+HXQRLa=4mfSIC3@)JYb&oa)jdp!Z$T~ zGsj<*+r-k%1Kq{asz6s-x<#Pjr;K#~e#)ScA%0|tAD%4(5BOGrrry?phIgAl!|Qn} zUAf2Yvqi(jPkGGWHPl5n(eP{=aC8%m?%M?%d7{yM`+y@`H1d4iO2_N*O4p9+QZ9b# ziXWMx(arPwetA7Boy~+(S2Q|{hrUY=wvv6Yp|@?N8wYO_`#d`0f%aojnL`IZMwMty*{uF|cID+Ripr5yv^#?tnI&RN+A z|J6SGs?q)oQ|Zn@{%lj}&Q1QjRO!xH{(M#G&Sm~QR_V@h{`^*%d)wsr^IoMpCoZr& zeSFjYe=E>$So-@wzh>#{f#zH^KhQT>IxEokS^7YrXIgrHpzpEt-at>cbVi`>vh*i` zzQfYH1AUvNcLsWzrMCzAW=n4k^bMBY66ktMZw&M#OD_p@T|{4C^hE(b&(aG6eU7E) z2l{MF$zzAJ?ElGuo@nWrfv&Z5e4xi!Iw8((t{%UfI#=NbpJs2wRB*hYb@O_(0wAho6+3^zLTYU1$swI_YCw7mhKVg?JeyU z={r=1{X{;=Ci7kMCbhnA4@nSyk zni(6S#=xqe+ zvfm#(3LiYIV{tmAKeDJVmD8R>_0j~ml@UYJ)z{frVuL6AV z*c59H@Ub`fJ;}rH!NY#003Z7qybAEa!yc#rAA2CY3h=?hzNr8o`zE{!@WI1gs{kK+ zExZcw!NdNn03Z7^yb6>vZtyF>$C$yZaIg{V`_%!!4-PSUKa2ZD7<};5L>PPnEy7En zE_^i>`&uMO-u_0zOAsGC{T!0=9bkEQ3F3pNze7^K11%3PL45EGa7fB`kmcbeh!389 z9Fp=4vOK&5_$bFN^oIaOf3XR?1h5k<9c*!^MFQ9mOAoa;9KeG3;5pnO9;5s)i|`Y~ z2hWiXN%@YjJiG+)!E=;DQodo9hnFBec#d&M%6GKo;U$OO3 z?;Oj+OAsGC=Q||jJJ0g)62u43g$_yiF0eej1o6Rhkwa3xI?KaL5Fb33I3(q}*z$D| zhF##nHU#R@U+e-e0qlB9FSYnx01FisFS7_n89~?-qrnMnEkc8-E*SamMVRV>QAQB< zeWSq%)P)9%<&nQU!c-TGGJ>!l7!6K99yC}ikNg!8rn+F15rkc7G&liy&|tAV@>fNe z>Vi>55O%fE-~{ABgT?a5UlU=f3q~11*tJH36OacD7Rw|5!w6GdFvQAQAk|9}&a2Mrd>n`-%bi#J##m`^spuXB?_JO&>;H%HiyjlRX=jS&VP zJkuf!zFRHAOP~$-rdhnrB0=rmZZy0E@xgP4LsGswEe|h2eDK`mkd*Ik%fm|$A3W0? zlJfn;^6(PG2hTkYN%>}29$td@;F;-=lLTwki{Qdk7mPB3F!GN(#LI&QQ(ff!%p$lj)di!BAdLJI4)OA!!BiJ{ zb1Z@jQ(Z912*SwEb%>V-4W_!tYp@6|Om)F1BM2kk2w;J_jTXUF7wk!kjS;50V3ZMr zk$=h|ULG`9Ebr;SBTRMSrHmkqJbpx|um}yNx?s;({JF(nSR{b`(jpjT1Yz@x1}D%4 zG??mwk$*PAR2Pggg0Sa|1}9J#8Z4Ga{`m+~T`e6Je~FF}0pyy=jX@AsC6mmofP{@{?5?=8#2OAsGCZ#yLATVQ#33F3q2 zPYy}>{%H9H5k^1Y`E!KP-*+s+OOQNx{_2oa-d`*aFF}0pyyuXV?_JBoOAsGCe|JdA z_czPKOAsGC?>i*r`-kP>C5R6m#*P3+KNvH331A;u{K(?JEE2%}ZSnsser%B-K6pNH zNXqvg%fm|$A3O^klJb3Od3XupgXc4cqAMwP9`8|rsjiXky$Wz)l9#@7k;+TovGDSEp8`yEk;nTJ z;KC#?efJ`jm%eM^`NMJ6~wZNL*_dJiVnMek|&cW!)L>ZI>%c#M4dPL0Q~2{y&H zY8Q-rY^zR|XAO(iwF`;EyrZTySA+j0S^0##qC_h2@L} zgFnXD6TpSB$AH04FgsU(vBx+(VC-37(Y{py#-0`JM-}XWV9`G4F!sP`f2&|m1&j7o zhq0$d`(*`tG+4AxJB&R#+J7t9^TDEh-(l?e(SBaRc!5Rd2!}D2(fOd_Rm;z}7@c1n zMm{>1R6v78=OKrYkIp$2&|uN|%37?KgiSOWoB&2S`8taPusTb@F0x1vM*aeacnlis!VE_K zyaJa0@yf9&#;)q$j5cZSbl8awZzp0qb@QC$UDu_(=Ad)03%P`7(2!Cr&@%c z03UgHV{DY=M_YuS03UgHW9%f$pKK9+0(|7*jWKM39VjEf2Sy$l1h7LbJ>KGR02YJ| zHX57&MmhOo0W5$)gMACYf-v$&JH%tqV8>)I^1~tw8tkYHM*fHhg9bY?gB@=9!!j5= z-~{xee5l1C02YM715N;=|L|ln>=oDD*YbTV?qiWaT`=;404#tFu=F5{S&V#Kx4-2N z47`@Ox?t2r1_62fEIq&?Wdtzt)Qz#emfznZ`~>*O!y98YmLF&legb^t;f=9s%kO8g z#v%be^6=JJB!FQP>_8bo7}C<(5FADx86i|`YeY|r z@P^8H(ZeFVAvlaYGD4`>+K62&!W)9a$Ri_!imi;; z*&@6lIE*|pLa1QO7&~}Fa2R=Hgix`*5$js43IJmcMt+M3t2DYx0SunF?q-(XJn&lL zdE%_8Zc7#E9G|`-7U1hX=VWO{^WMQmTqu z4(Ctu`cyQJYN;x%FRN%H>#1&?wX0}Ufb+R1Cx~y+{0mJ7vD9aHV*Vwev+@>~mtOFd zL)ijl3zRK@1y*%ujCNtuVNW~IqWw+m>KB@tddKTpx{lfYzy^^9)lUvEYEKelO1r~13^*H~8lKEsR}rir1=cI&hGcTNpw zfST%+U%lPt3_<1g3>nT>I#20*HMP$Bzs_C0UiyE(A^lDpdtz>Bj#v0W{_v;bW6J42 z<%070%8#4IF6$Si@~RV?wM>^SP`1D~+X8LE`O5DcrOsDor?8Kt-_s1XD|)_K_r72D zi6iTm&R2Dl%jc_fJ=fZeMMZ1PE|pKuSK1#7ov+eOHkPTje40I9El4hG*y;G>ankX_ zlgB%U^8nl{eu!-v(`nCizEZ!7ov&WKeO&YT>dL1k6+K_+Jf-thr=7>HG~vSH_gI{- zCXLV8{S`OCOBPylzFN4G*QflP6VDO(2KEXExo_ec}nN2NB8jd)VV8d*;97D8b4{ms4U#7d~42E(|0eQ zuhO}PnX`UTDoKc;HuDI<{#{r#;g7O8qW&zG~_HmCjQ- zUybVL&oerAr7e4^-;n;N4L^JNeLgs>rhL9i=Ne;>^@~z@%F3Z^fwBe47Kkm-{Q01J zjku+s59*!cAgR##%AKW3ov#M(>DTk}`Kr{qjfG29j{WU#Z_qd%n_nO6RN22YP$z+?BTMDb818$BbO616FJ9uR85tK3}DCjWNjj zMX5Yxey#qYQG=+UEj4XJ4U)|8V~^n8`BQE8jHzsmJ5pRZE;Xzj*M zqP1q1%BSZm?T>}dSE==5nQF_Y+54;P=YvxZ_-3B3)bC>FtCoH~sPmN0SM!E?d+OYk zj*+MAd^LXL@Ci$Gz-rC;>d`~X=c{z?X>MrlEiSKll^w-$zHEWA1-`i!2=`YV{O(%% z{;JM>KM-53nQyS7=c`ipSF^(Nd-?uKYq-|v*5uRkmG;N%`HIg5{pd1iaU1#h?D^`Q z;rGwo<&tsLumAhPhLwMM(-Q;3d4O6geu!-x(`nCif2DpGJ6~Ci@Z7=o2e&Bte!I?7 zI$upbHauU1d62g3seVHS3?F~$$T7L@{Bp{Yh1Q&}Mjc%~UzN{SOXg?gT4f8AEl|h; zOZa}fer(8#EPB4mU#~o~?ypWbEbK1^a|S4XZz(?eXwS!Hq6?L$=PT`x+4EKU*`Q<< zPP6B$?ETfkWBhngd&Lj2O=CLkna)@0cd_$TOYg6Ap3?cM>qu`;ox9SOJ!R*s@x#yJ z`-4mV`&af`Qu)K5ZN%r@`LOdzR~@8p3%f3PsOXrS7lR{8ssVmA{5-jV`s0WA(T^Jzr^mEOfp~f3q=eL$Z9DJzr(t zZ=XKmn|Z!czl)u(TKayw&Qm&H%|G4SQ|B&5ArAe9>^u6j)4B~9IdUAASW9%uYR&!C zyi?2Pt8{KL67f7+Twe1k9>FqQwm{he-)svszu)f6(9-jjem^_duITxSH7a~>u=)2! zb$`_up5M!#54L6<$Ko-6dcM;BSm=C}USen;STtSS`D$1=4}>va!IOL&rZ!ExrTZ)O zduh*CI#20*HRViyp3%80ZP`)ynMbYwQghKQswFSO8euo zI$w1P=Yjt}=1OeSm`;1A^OgF&wC5|Gr*yvRb)olvox9SOJ=Jf>e&dHP$KP-7dVcwQ zmCiNBAnO;U@|2ZB*#cz?lr0cjppE-`I6_L@U&VhPM}My^)GKzr(w$ZFd-3q={;Kk< z^7$%%4c8i7Y8}Udad~>a(*C%t&R36K;KvIMR{W4}!_=l}&vd?0znAuWrSp`|SMOfx z?WuED+Ontm4cUK8t=(Xaw2uf~+WPCOR{edP`QIs@uk!sM_7TRURGzYOC|jUxfwBc+ z3oP0FmH#ZMSx%AjRgK|_eD}~tov&UB&+p~?t5WMWHZE13p0Bh&F01p^sBj)=9`zNP zh;16vX{U6)Qoon>e5Lc0&R0{f^5+?yyV90D)o;i_!_OW$ehE%hsif9?J~-uy^7$&A zYuGUB7p3x)l|$JAWeb!o5L;l$&R6+gAIudwU!}j_9=E9TRoxW7o|n&8rPggMSE@Wc zUul0_R_CkE;XDwJ<%&zkHjU}DM>=1r-%ESG(s@eftAX|2|8?$4TlSQluSTCd%HF@q z^~@HvINzG{Rj=#I=c{zCVdAV`l*&_94rL3JEl{>VY=I^GeTMuG2#cPt(lsh=Q|GJh zSC`LMseQC|V<*vCvrFaE^Og3;Lg%a0`ms#4<3M-cmkarE`xl$ofU8JZ0rj zwm{heWeda>X!-dn|M%Nt8M!4oUo8mF@8$ceQtLK$E>)hMue3idtMk?5a307T`4u)3 z+cc)r4(WWQelP9$O6Mt^ucqJQ&oerAr7e5P&R3I8n>c#hX=i6kTApvs{ngazi5!~uXLW$`D%X8i7QRGFqhN0D{a|R{e~PoX~fA( z_2+R~^?v)n2g>KGbgnT5S-&Wir>q>x7ARYwY=PJUOZNWNDzS*-=c|(6AFP>CK40ap z;aa0ht>YLzE>F)_+8+y@ufB@kzncG`AFuqOUr}@UHcV}r_DAO{^}E>ls-?d#s`Hf2 zSCjYl=PRAN(w05dZ^+=$V@?{?ZP4(O$BeE$dx_q2Z_WMHu!qX$t9(C*eS|S7m8Yy6 z$`&YFplpHI0v*ENclK}Jl=^)JtJS=QMoy!wTvCigoGUA%asYm8rU(eDqA+s*GU z<@>8Tvy&pm6HU6fe0sjp{bguOXgj6UaC$yer2j`fwBd@=@w|dzp|w`muufDvK?5Tc3hMqKj__&eX29F$m>VzdbWwqvfHSfUk`6``j zj6v2fO64glhq49A7ARXFwm|dw%7HCCU%?lJB|2XI_;9qSL*lDp09MC()nu25&k@*b6483r#N3tI&IuU_g9{?g)Pk6 z=fden!^P{kv%%!U%IB+eu3@aKUzEyIRt{wglr2!UKx}~}J72lWu2jq-=c{RF9c)86`*ddH@lZn}XKrw~XXkc1>8B$SZyet+l8JonzUte3!>|L4Q|-`&r-&$MUi znKS2{IWx~-pANeFZA$!AYWHu;UnPDj@mJ^N9sd*G)i2py@K^Op;s4zIqU$bM;jj90 zO=GYeFOKC`+{)id53KaSN)HV7z{%16%EOMOzZyC}I4l<5jK3ncWZQ0h)?kT=zdGaK z3V$_B!xN1@79Cel4#xNStF%5Ijlb&m|6my@FMlrQua@6`b=_lqy!v_lF1v>Nu-~Ws zHYNTlwfmO%t6%s2tHe(w{_00F-k*u@>X+;;@mI}e_y3sv)lHQZ{%RNx2604Vax8zE zSIb9n#lO-6D?RXkZx1B>mG^+I4Lu*2o}QLH)x($3luSljw&`#S{M9DH6ZCsH@mDuS z`F%xybu7BAemhotpTA1$`l;`ksIe)cWe>EHV0e}B@{dcfW2i^U4CH^Y4 z`e_Q@4@l%Pv+PUQTpZKnR$?k%`YEJ9(IA^V2 z9`$Sc|0nkC=Pj)8SN*xBE?$lo$MP#~@}q{Wc~3Dz*ET_^V%ce=zY=iNE^AQ@lSD-_hojy)4cli$7UtvN)N2`!2jQS;5WR#y}#k44*HGwtM5noeMNtDEV`}k zJyv|5ze?-l{}z9>82JI!=y(5buuliw{gx&EDz*E!<*yPymH4Y`p5y#X;=B4KyKBq7 z{guVv=I|idFinSN>heO|Z@QPOu2|u(`g2Vkvm7sus>)vs%RmH4T|UmbdhYFaD;cSC+~DM1QsOMJxPOf3B(f zmgB{-{EA!oTj_z79$4vt!5(n?E8iPBHh;xi84WpcFpFE(U!^mIcCUMSCG8JxdhQB; zHB7@3jXo9~S1%66_xY=|J|2y~>MJn@%Sd_obNT+@a{Kn1UhLzg)W7?G!+qHA(|*em zf0f$(+wxb5pGy2yDkm1FtSyh@|D@?7bG zl^$5>fy}<$462T$zjFHGJ+@}J%-_sxaJ4*9!B6nqWidF10G5 zgY}l4)3w4(XcX#|!{?rTN$3>jE9GEC)UciJHoJwI9MoW;TBf{eF(y9ulF%xYL$4F6 zrDh{fB;F0GWV2OicdHd5c%fP@)|#a`Rk&8Me66hYXKkP~g|!OZ*~9LoG~O??D~)bg zIK1JKFsq-^Y@snzDGxaF-D{mte00bjA&trt3Tp;N1tD|z*8bkV`3UR#|y zUT#-;)4Ub^hR$qrVXm?i>b-77*|nOT(5}=9UDB!8O07~4;|rDQ%xt$4=52hu|5Zvm zE2VA2SSZsk?aH)r)SI1d+Y+0*dZgm5rmrb!e7fDNs|Y3T)OzjKtVXR-A*M-oG)UvA zkVZ4mi8Qg&u)$kzxX=luX06#i%ttLPM`~?8QzPr;)U~Q>oR|-crC-qFdW@ zl{T@xM!A_U_NIe5)Od3kB9)r;mQW$oHHx95`gbaoxlrydQevkAMzq1Y)@&h^YSk81 zw5KXb2A$AscW0aVPN`6mk9DzsFp1DY>e zTa7}uRhX&RbY*yp6dD%Wl~!2NPj^B6)Ll?7DaufvqFR=YNmZJwlr`O)ic~8LmT|Ae zl=L!s9mYhp?6fMlW-1MOQ@>00YcoV%t3aXCS`7Rw1sYgtGV-l%{B2Zb48_X!#(1-M zDbq8IlP>h;Gdcs6xRl}McgcQ_D2rCLF++EE4sV1IffstV=^Ea(QVlWdOzK)_d5Qi| zkqsett3yL8VPUpfn$0(RUHU&1+r7?gp18c;3=qRm3}9PjI?u!xvQHOEq5*o4iyq)f zOV5Qu+nyw%S4s-1SKF4v5Y*Fhn*tFjL8V^p%0F9Zm-TEHAjtj>B&E@@;tKVm;>b58 z$H@%0NSu~ZDaxR;o}hadN^Tj(WSTzmGbv`evvQScy}H%63~5xrT(vQ0uQYozmVy}_ zGn{D`=BwQ$%b+@K8L7+VhelaFK=zt7p)*@3Hy0Fjq|)eF&Nv8aC`|Okl8r;C%Z6b_ zFvdGwJv$H?))}Z=L8L*yU7H0R<_?5J(@N{Y;f>Jpbxh7`_kQc{hpXBEr5hzCTq6ZB z=W9h!SqUJxoocfY3ZmDJYnqu*rXow(B>_(t>Lmi!EShKIC5Bj*cAgpFB&5;oGIOC~ z#Y&r6mMbmpJYq>HK+rTNjl4`#Fmxbq?tw65gp;3qsZg4&W ziiJ)!mby_5_EZ`vo7s_aW{uEnk6~6*mhe)uC=MgGS&JO=rqXHFl$~)Dt=JO~lzd!P zAao)=oq7{WC*X_@y@eqix8zD9)B`lsy4nx#si!MwHQE>37` zkM#wVwb-jM!?ldk{SBrK1in)sf~6MZun99`SgIcgAd|@6tN4od?UZGKInIY{cg11v-MlAEW&g(@_Mx{ zhbFdIE^EQS5cUIo>@41#8-&JA`(Ue#$1CA57Ecai_&fx5M0C9bP~%N(K$&IL&2di@BMd!T^LU9u>Yvsa4M0)&YPOikUfm~* zk**GHr(5;fU?^T|y@+jGdYIHTn`cTZBGBk)(iFP&yyjuGoP5x2U#wWo^KrD53If3g z(={yzLxqe+EoRq~85(vuTE z=4Pc`E!1M!Ffhz1ewkDJ7LAqB&y0SG0iL9-A9KyfrH)}X!IbF*dc_b~+zb=gIdr1w!X&`P1h^oS{-cQCQZYl5W5Aij%BgvQ3C1KfDW$YITiBAA)?!9^4kHEz!o5$|k% ziKH;kL>Ba{iQv^)(?tn|ry^?zH--|)jqYJ)*7>9gt9l6%i-4u+*{;KHAv0R!IDswA zEhHqlgmT~;!lEsl$(aTFYNGT?mI^dEsdmWO=-DqE%N*Q1Z~1dRz;JerZnm{9i%CyDluGStc`?8aHNrxe4Gq>XdI7D8 z_bENyk`h4S-VN?5R^}0Dk!Oi+yNgAq*>ElyK^y5oxdalLdcsDutF2HMK-~$Oth-e^ zb(oV9a%$_fcjh~5vp`4sV#;@lq@$_LnnEfpHmAc9ptp={pc#?O20j&=i|~Mk7SlrB=|a6) zTRJRd-uS$Q_=7$(Z#6!PRj{$8UJuMR)&r?9n&=Qp_89LP)4&mi^of(D%RhJ;nGd}93-5al0$}kSsUG;q4t0m!`N5_P7RjKYeVWqYgktj<-9%>w2>MjB_5(pL*^bYX>U6r84$b;z! zT$P2|e2qrTRP z44S^GH*e~^e5SBU0bN!+ULdJyt9;bj(;^qDJd zAWEoF>Y|%sT9CAg`9d2#q4hbc0nnSN;0~iOQe2L^Mp909*9O$@J~?+ zh%Xp2dCyv1x~QM1flU=->P#()u#TkLm;sScZud;|mn-%wtXOoVo`zV#*!deYbEe(u zcN_Ygy2R)4=^6{NhQ0=lO7DxjDZLfzjP&VZM}u8 zzJ?4{7YbxZBQwcyl>9U*B^1N6dmJOjJ9Iobz%yAlCbz4JOo9llu02V?=FcLthTmol zyowVk?_rs*A0hJTlagNc`G~_q)JAl{{;*7o^Ruol;I(o+m<~_7J}!6Ybq2y3lT=eP zthUA~j7uS%sjR4}{EnZ|Q(~-3^d0ZZ>0}xfOj%IeQe*IKyk1*YUig@p%u2#tVt*p) zC8E!lGnEOOAquL6OWw2>V;&X1&Ro;IMo9#1-1K*%biRt1D^dHhuQ#lrRYPQ-)%nbC zgs)YI`#dntEEFs(qFf#OMxb2eXaUuFOFGnP^olSU!W}jbdW-wr>03qp`88Lmw6?-q zz!|bg&KV(c=bjccRnuj&$WB~N5_LjMl$wX}l`h7lE0Jjrwl^cm(4^@*^Yb&oDJpyv znwaE48`k%<+2{}thax{c^9nQKW_6KF9LXgjjXW@!%tNwXu-BZHjGOk4_oM`L1UIHA zipa295^im*7Z|}jRH#{!xNlq)8-)1=eIz>2uAr}2ZOqY|}k(dW5do(=lhs2>r8xQ#px@2DMmbpkA~J@PIbM~|Z-#6rt78xyd#*a4VB zyPWJB&eI)r6MhV#+S0>LIKFg(D{Jjhbuj*`6{#*pZ&|)s8vI_8i8!MX%}?Js%GUjUJ}p zLGh>kWbq#oVf2SNr(_c8Iqu$tuU03Z+D`;F-2Od zgcXf)?!ki7^H`$P3tA6N>XHyFGXwfYMkJaQ!c5rf#K*uhM$yd%#5VIO3idUrwZv80 zuC5eGh%82=HTSepY(IGu434lq29$Y;N1tXJQXDYr4LmUsl31o;a28m$!8lVbfN}PW z;dzT;u@Wlk(V`ljkGTwSJrK^Z?k?B2dHjIeq;t~}78w6_%jl4$>J+VJ(T&B2=hG?L z){geWC20H3xa#YTWD&m1UEU9BJ}W;hWal-nnWvudEwb@86Av+2`>s)Yb$C4}h61?= z`w{oiu@n;Ci7k{uz;`e48Fy~`+lEpaUE*c?A)Z`Mgim@$FNqbx?x7xcNLhxu?xaG| zAw>X6Ot;uzZv=kI)(#@Fj2gQua$zv0J$Mlv52$z8FqSo&94;JdQ`Bg*c$)Ho z9eL(_$9h_rL8q3h1;^y}obO_^W|unElD2RpTFx{J*p^sva(Yr`j69lJ_GISRQ=*P9 z%&D{~@LW+=;&kuGktLfZ?7?}SjJwF9S;47xQx=>*-_lDR@B8_*MH{>PqI9Gb@1}7n zGEES(elUwNM6(83L#wbBM1*3P;u;z-rjH!VRgk1pEcvQlKuc`0Y6q z0ajps*VfPy*{sS;LAis!wI*Z(RtCAUgn~?}OV7R9XkpYN287{BIBUC}7+@qx4&8TM zbF#+D#?zd}p(PKcMQa2WOhu1R*QH`_F60a4dFe&UO(QkNsDSJuJBv$4Y-(XY!dfUF zDz%ys#F<_dQH)kBO^T+vK4>t6Wgp2BF7$xuRWh$7&wSh1@x(Z{8?`0j()W6#D?bD&aNjL)U`Jc4Y|o|$x2C%;>^ASAX; ztGTytl36TTIjROu$i7@!>bJY(VcMMwkdsQ-hH1ACim;2NZND^PT4|>+;q&H_Ky$6K zA8u0%fAa55>#>zjoPT#r6HpaWfT>3NEBCI(w5pP=%vhAm+WmY<@`tJ?aR(N7VHuVb7rdo{iXjzcO#BvPoc*0oyG zj9bzoWV`_l+0tB$<+E#U1vN_|c;N3odLmMwZ4DSkn1HG#7R(CMWo^j#Fcmi!OSm(+#1`anGeSVGKag!T)6O$mfy(|ZG#FtkTLHGj?mOMR3 zZV!xE3P=sGME+3Q3(aT@Qoz0oajeiZ{=QTg`u-Aw1edQxl|XSh^1>#VDGEUt%MAEn>NgA}6!$E}Hz|qJ!?v8G6$joK=3Awm z%sL2&6)|gASO%KZA^{>&0eft=I??CY)?VnKwnSvBwYIx$FD|AND#B7RhLnmPFkLFy zoaDLF%=FrbmgiTNwu_zE2Fb^$W+Bak^1_y-Zl$xg(w;JUPgI=m3%jH;tyNH>_)Az3 z%GR`GBE6egm&#tGRC1x@lkqYTc_@{6tw!dWa&1nkfQ7MY^NJU2!?@LK^ZZRDjb^)0 zOD}*Z%d#M3|FT3ZVAfolf{1eVEcD7%u!NGVHs+mTF_ToptT6wUYS+s)UnbYOuV2ZO zw*Ml+7a7AMl`9xTSD`K`%Vj>0bs&y?GHF^s8CA})#$1u*nn6)zOHP|r7D3Nml^fwN zx!q>Tlhu2RWP@!0h%`#I0q2dE? z(8_|CW*XG@vj*mmn6!gS%mlQ$lVo0Yw-?EVcWmO@Ln`ZoX}Up1C0Irny4w>o8m)NP4JAe=1t9zNkjx=EO=_#Pw|utS{Md^SS?UNA3wEoKVv{=s z_CwhCw*7gt9}t~HKEW>Dh+`DVnhp$EgR`AA%`hJ`YBmu z)af?a2Itq{^y<&9h=7`C(GH8A*vMkd{v!jcr}ecQa4c+T$R&e}lm-HYG^{{GU<^CO zjF=I)EhVxBPnFDTGcRpnh{#EOQRaPbmW8lt`o69eK@}`}8Sq>7;F363Z3x+Pkt6Q( zq2O^_20Ux>ar6WOIP(prLsS<7D3s?FP4(#X!rPS@W+x=ssk8gEA!~k#ZxGqG z*#clHR;bpz1%^(80p#r&iVlbq*35OW3{As&02ZT=MpbiDty5=4a$&eqS%OH=iP07s zGecKP!P2>|ud-ORb3#cEI8|l;(o@?`>`u1_K~cb{ViMkxT30&@C}9|#z6VNeRtOdb zbHe5cYzpFSoLi0oPWPlTDcU+Dy_V-0vqT3=7fAwdS0ruXU;{%gQ?=WAVXniR){$`{ zq|y4$?JNx8qz$)(Qc5}n)wW8~a%cdxYlMNOx=j2z+c5T6@<+s0z0GoOsrP)&v*ot>5YNAy{^M#oy^Er1!{Q>w;Qfh=$@A&a~?^sa+j))C-r0H`x zw%34B)ebq@n~0I|we$vOu;$|)K-@CStUKCLB1=3|PZfKvN4->%bQ`Us>@3Tb8TO)k zSVl=Xg$9zHymGTw^jIHKL+AqLu4Zq*kk4p$4M`bnm4GDj0v&Ku6mTBn5!4H%RNd&) zsSC>~`lAv%^R=Xi?E7pNFZm6{d-J}|^Blkd`J52u-J)Oyb$Y3qOc+${DqCN&7S|MJ zId{ZO1jn@1oHJu+g)3s2JojXZjrzFINFHSejz)b)^jPi88i4UuZ7}X}QqFAMU=(}~ z!UKs%0TOpKEg;*Ub5k{`lRL{I7!}OcvIw^&5kptyM4zRNK|Wv}WM~}?X+6Z`8lUb% z;mLS;D6kE=(q^>Vd8DmI&nSzF#YR+H7O9EKB&iI)n-~vItQ8&9j;)A3SXWL3BJ)_& zyen;A!VijBsP%Ll>~T+sav!P*n^;7hlJCI$mS#mthrQY!F`{#`+_iznnV?u~*2duq zXTjReNHv5bmhPOwq~P_8$!s7@i-zM%=AyURn2x4foklvZW-db}bNL_);iq{#Gi|bD zXvgN#r zMMg>MLH5#6Xs;n#D|jlKN>)%zpJI+e(vF&gTPCNsmX*nRp$%vrwxQx)X0AefKOBcy z^6^AY)#a<3pc)i3U@5eI9lBH){eV+BIEW@-#DrwMd{v_Tw z?~10tBsWG9-NcY5k3eZh^HPF#uh`~D#ahMpsdMj*lxiC}hpd*Hx z#VKqweqIO*;S6r`FULC?u!!uQP!Xzx3s;%WaV!~aB8T(|+HZ$Z#U}h&AqmKUO zmO_So{S8}|7fe3VHkf?5WN#`htiLh}MD&CDw1+7yaz&b-P7&HxK}3sjS_K~Sc0Y6C&~dN(~%2o~Gp7O1mUZ7gP8 zbBs#K^mFVqaxJ2)_^v79(2enJey-+j2zR zoOLz)HA%>sEw#hu2LprC(a83#is%TKHr6N6tq!r*42N}|ST;YxcxG8~1~Dm44>>uN z8Kc-rRY1`B0#=2o?G+Sqix zf{eQ4>8_8Tbpec8f$s&jjax$~mNjiZ0PPIw;VfR>nC6HC4T~n5jn2|ShQ$DB;YW2M z3nE)$;3Z+b*#g$or#wx3J*o}ED7^NBO}53v9{FCCnFR)L2v1K-A&1PXoF8JIENZf_ z8C)xy$DcB1hSPTQtstjNQuL%#1A>w7N6&9yoTR;&nPv5z&}mN*m~K-GPNZFE@TqMT zowWVxs5c&)u$XUnHN>zQ#=)LIR<*F2=q0NV>w8JSx+?;nA0Anht+E)i(200QC8@r& z1=BzUUPR{bxrTv@%ZBiSh}yC`MyiLqmW%0ve6rjx%Ioni|LeexTN0U;9DzWl(jW~E zU8NzH!~iy=5J6y8OdZ3PH@XS5ZP?0aE%z~rU5f02cqW2LR(jCyOnDRH`#!(BLze13 zcViwhr^NV}se<(!ZO}A9`_FlwIo+dx)7KyzOdrU(e$8Bls-iQWP3(juRIBr2wI8~SV|&O4ZwZCB+W|09#<=JRh1xr{OWu(2u+9C9Ax~s zt^uZjBFt{yJoJ%xEE`t|aaf>>653ouIOx%mBy*SC4pVUrI!wr2tFU>mk&l5w)Wm@i z+F8Pi1wqZK=?4*|eO%BEE}^VMw$wtUMT@ODoEPJeuFWM}MaYCsJeX-b8+&%pRYyA6 z&SY>SCNb!UEy?fVh=ki-9kdl`h`Ws~H+lh;IifgE519*TilZRz#Cn+Z3{r0=)-B9U z2Flux6{fkui5ga0c2Yj+kSqXZ+v}wLPZGdX^kkL9tRH8e1!`n9L1AOZ$rHYE-%2!$ zmc|bAp~X;#4ybPoi}Yh8nz?6IUDBkWSdgtDeLF`JIW2-*LcN z0$5EJveIS$q^_9pEZpAmR=m`ZaxT@s%*{Y4-W`$f1&A{h+W zXTeJh2SmZ3{TM}|AU$(>8C^2hg zWEXRDe@eqz^uNk(j*L&vLAlEEfsjjtAT%1C5?$FZM0WG;C z?pcV7GIIJVFPF{J%#rU$7zZc$Ns;2vI7yzlgowz`3XqJRp38#q`dLR~=4 z2r;GEn{GKTuVT`3AVURnO_7%=voO{)^aM~h%fmt}jkENs*+3XuP)cT8C{Gvl72++6 zvcCdc1JXpC%Y0B&?^0PxDwH)4S*70xI^)3|Vak{jk@=n$;>9H? z;ruPtvNF<}`GIoE90;aLOlp;i5AkClfO(FxkDV#InMg@ehzy#nLHz*i^#&5yZoptb zl;H9H+6ABrQxRk0;f#|+HLIHLX8I215|kjuhNr)Bq# zse2PXij($LM3~w4Y4G|!n5O}sSd#&7l5^8^F>a+Ot6yuic}*}>!+fIz)fPsUcxRc6 z+Ny}6ku4w!Pky*(7-l;PdX(Tqc9x}9*Bm9a=OycqqeS*NhutE z0F~V=mwp((t!zPGCmLPEQf(e!9XP)cb6KrlSQ2po9V$aKN3u8%UHG1WOmdmdj^Tn@ zAr^3=G@w{ytEwyG$-t3Vtd7`kfL*4k(*aWvn7;3aW#uelsdqy`k_~gNrk(F4EbqF-@n8q zEV2dioK3No`D}KWM1tPWFEa`@2P zW+Y;+XhmV}NYdC8YE{{|rGBC)HpSt$WJ%oUW!o6*hrL`Lpz&j8$oO+Tt!ZM+xwI18 zPz|M4G!NTq*jehl^?h}+rns%79?OB&hGQ%Bjg~lu<6o?&UtAI+6ZYr}| zlLc@`6oNNVnNqb|WR0C>OO&F)YEG+hpOpI?^G$P`pOs;-uA^IUxr3(@A?rJQ{sfB{ zG>DK83KmuK$og@iE&(zs%_V4MeMA@4O^)*o8_fbq(zfJAs}kzW;7hx&6<&(zsLN{> zC7_F+=3of4-73K|-iqal1ygn)>)UbjyoMgZeuf+8;C;2@r`5z>q`sKYqMnmN*aSu9 zbY6!(8G_ha^MYR5PDWKMrHGsh1u}BchHVVoV0=28q4KGt9n@ZbE{0I)_?T@^oELNi zl=0AQJyl&x=K+SfoN|zDr;`Hu7WQA-BK5*USU-z}Y(HsmTS2-3X(4HkE7?(T4;+1C z$wwJ?_&lhJ430Ryh!um--rKw^3~5qC*OZqc%H|LOa&;!X;~IB%iW%T|iH8h*zyQoa zP|1-4H)1MQPllJC=jgKsr_i6S}y$cyj} zl@c?>NUaZ->8*5q>pYDpaoC;%Y+MU4Zi8zKxXI3flzK>_a&8aO5=(;XOb^>fY znf?ckg8^Z(y2GyX@ar!|?%a5bHcVIcTexlK}Gw%6<#tcy^3^ zI9OLQScz%`<>^sJE5;(|;Zlw%!k#i1W4GAk3?q5IFf$=_2`xi}K<6i|yvC?)#o@By zXEreLay6NhprN(P6Wz`B-(^fKewkeSZbT`2nG*=Nr;Pcyhl4U`N8Au=Gv~A?*Y5B$s`aR9Y?*TO3>XXtErLI%|pb6lN;(B zO-ab_Wn!^(zb)4-o1R@}yoZ+ZAhC5n08E{kW9Xi=NT8u}nftmPVLj z%SyjG-mfP_HhFawgxZK$&wP<|YN*AIjwl$%cL-9923aIP>GcFjYbK%kCZw{IYMxoH zl&S;zbu+!!IX~kDN#b}XhkAU2uD_-G{bW!cGH(ixjCl9G`FW?9EagUZRC_Rw7V94G z3TC}J4=G6;Vgh$$ER(v4tzR9_6%SyBy7mJnXBHtBq$Q)0*&61t)9p%kcQ zIo;4nv+R<(>@^)E@`X&+3|T8km$`LD(Qr*#izHub(URZ*RRI^gp;Bw_F*67eU715z zxK4PQ!|#nn(+A%t3tSC2yfcSH$8~2mPd6XpvZruqGu{lP(qZ+?rqOC*r~}=F@f1u; zyel~=7R7g?_&Nu(7Qj+?4LeZ<Je#wmi9vpYUSx32!bx!9 z-VThD;~_ntV%j6%6`O9%e!0ncy`JO>XOIr!#EhjZ9t>hxj?I=bo~43o7<6tEQiDI{ zPH1w$8+a*%WiFX1#bIGYp%ri*jE6Q384EmGiDeMO9LcZGT-}5$wmx!B)g{6cVnPH2cC&e5F(Oc9PQUd--iJh66h}wg2oO<+G zX-A#yh%?$katm}#RLrJ)mag*{_pUZn;a%I=?8GNb zB0H4gDeaj@(5>{`_#4)L04cP1EcYfl>1m_~GefM-G4E~r zyE@0vMia;?XMP*ejkyC9M$45WkzCyU2^Xk)H?Td>=L4LON1F{5s4opvO9KJ)5!;TL z{gwIX13yL!>pW+yB2P0;h?yr;wRMEOFE&(kwIN@UVBlz)Ke2(AKV~(J{kCoSLiV5< zb_s=Or3j@IFLB|;VI4hz%vX}f>_>(x>8e(~2h!%&LkN?9*+6C!)B}5P$~1wF)>{od z!iF4b0e`W}uRcIn5^G=^c3eEddq)2@BF3{r=^LRSy*n!CA$5zE#?&S>LlMsiFhJ3z zE0$ORAlcgb5liMTlVXpqnG}zsy+R&r@cKnXM)r_;X^Y_@odR`<9@wXvOl8?_#wH|D zU~82^eBEUEMwEsKL;AT$KNp9>;vwK7;9?cC zTgoC$K+4P9=w#Z3?MGPR=KB=a3<~^{5UPkyu)@tWA<90F-0kzouFQ2?NG{LyF#3P9 zxS1oQ=LfVCIev(3B(NzeX3v)pbGnI~Zm)|->E}q$v`9_X=Zf4d?waefnG;4K<3aaBxl#(LPg9h_fO1E2+`Himd6=x?0>E+TaGy>US3^0Gx0uJAZ&5iC}1?^?O ztyL9yPZF4{Dr6{T1V&ai*t#^iKv|dn*u6)_L5rrdoH~hgx8)4>$3QwWO0PZzEpsq` zj$7-ckbtXnJ4P-cK-FS3Fg3jRd@RVU_9h)veUZ#Q5rVSEI@_oF`Z(=3#OKENJe$iJ z*)^yv>R_0DupvdHE*n*Xy~e_+$4{$0kA5V68c0oWdsCr>)du#*p9=9F!XQnJQLuCs z;+_#9O?MfatSIY_ag}T(t`k#X+wi)>Vx61MngYlDoN{Fmr-JrP39cd4tpaU+I%Tf= zGzkXub%z8DqsG1q7A>cwYrs%Lhh4x`cNh`mT$tZM2=2N=XXEB7v%1#?VaoRiph@1( znKvqFqE^@Kub`6X-eh)V=1ZOG3^&7=(gam#SLL;eOQ7$<$7fN_HZBJ@)1XY8NXSavz}&Fa>bTf z#8+S%&AeGdQZkGj-J-L8q3CLE@z~BV>K$(W<+f0e(Y`&PoHZqNWZUjQGcsu^25K23 zjx&sx!T~cEbxcb%#crQ5Du>=GUz@Q@qO5B{2fH|dxuhntML^e~i^A`=tUdEf@>Tdu zd=@2sLxB8dPwU;xr%UfqfG~Yvc~vMJbG-B36Z!x^%gZ9|NsWwFOIsuLUPlJxQM+tc z&ROqa+M+8b!5b8#C@SbOcSz6eHllJdDb23b(25CVX+hJ9lmzC%Eiq87MR#L+LQn^> z8{D93X2<-RnS-6?Qv>XXpB%*nvXR3r0p(&n>mN(eJjzD{IdezDG;AZsQgh)36=mOPH8$!FiDNuD?Mk>8 z%!CnSu)Z4&on$!SG6exp4H}X_YT*JvysSgdnU%{B7B9qatIVF^dtBrel z@w{7H+A(9c27$R=gksCyHs7v15 z1WM8dtE_Y%9$@rjz|XAPn9hO3GGk$Gm%^qGr`?jM@8!T2!TNxLvgeHhX?HE8Fe&?B zT;p))vO#8pRhsh!(MgMBp4s*#R!;BF81C8*<9^}pSP7VbwtPz$V>~fK>JCDD(Q4@! z3Pe6GqfizjZ1YAJ>|S>Y!|+`eWS}$141sW3D1Dv|zWS1((UsZ@t&1^7V)aNNJ%7tA z6c&{6v|YB0!olxXVX|>r#tgXlJd{+!`*(R8^?z6k#bqY#A?${KK+ z#k}=L`Zh+f*IYvLqvb`kDxH(GHo-C2#b>V7u;Uy?w;*MP*Q}fHPG%Kp8lNI8!$*h5S++At{sSOlP8?;Lp;Q^ z_RmqHT{0}q&ScwU&V07(VU{rcSF|B(Tf35OkP8a%@W@C7&1Dljv;;C+Wm*|DA4!$43vZk@>pi((wl=m zE*DV(CN3)nOk@=FRAnsBB;WY%~hpC_=ghGwQ4fvngLKdKbTBvNK$=)v}j zov&hP_p7r^Z{~{(b5vO2dH}n|P*;S5r+T#Pi=4_*+doG5NRPWaYeFJ!>qGZs!LkSO z&b0{ha&(K^v5eqB(V%eA4Rc~1M^r17-lr&7QT8mO)+yc?9eq|N8&2@d(DWWBB((8^ z(<01aSJP!dUcXpGx{4VhK?c$>IrBxE<2OxU03;#$&Iby3Ik8%p;oLmSJ;=twAeP0t z4xUc8PdcT8O!x_AD#LuyrH?Vd4C04_>_~<>nSOz&F{KG`)FW?Zsou5{7NM^u-fVHEDBHA(35O__N(M!&u@>UrOz~9;$}xg`BmGhsv!9 z(al26!EeLE)FSL*TRO-uc2X?7m{hw=4PMnQF}Gb34r>rg@m2UNG(p(!XH}Fh#pteN z3^+}xla$WnOrXD{sS)SOfK#Q~!F%qgD*2~ooB@l5HCpne%C%J~U|_^TBw^I|ZFG$U zn*~lBGLP{J}W4ppjx_boL#Ob3igaz|h>KS*`+L z)v+@Z;<)rNUK%AEr+CeV?}Z3qFY8t8a()%iVS9?tmkbz=*dokfA9^4DLY7mRDnWjl zEXyi)YRl7HR!6X6JpeKQSv<32p}PY^jwQUSVe{J-b+o$Z08o>M-u>qdKy0O2x-1(b(Go9h=vd zCBhIZ4Lt?Vk@`59+EmN^da~;fl`HyWEZ-;L)3yb6Uz?m6Us4B>n|~?-*3@{J@s?Y6 zsFrTjukLi0Z2akbxG(cly6jQl58W69$6TFH#+Fe=pb)IteDm|V)?tZrb4%4hpA zsFLs>Da}$p-vTHD+Zz&~CP{(G?$BTjnP zFVZkFw&d8LMe7p2WlkfwHa!W5?V~g6Rx3W+v za2>L#^^(*$m1q&%JmF#lsq(t$R3@(I0U;x-HnI^?Etv6Qx_rg61jRxFMv%|h&6pOD z41;7c*fLgFF(BsCB0S`Sd(C1Xgyu$z*YOKMG^4Gs{H3~K*k4uQk_47}l~op}yv%d7 z%p?%b#RbmV#0Yl8bQvR(SUj!c$-TfjiY%T_Po0|4cUI!Fb(V4Ywl&T{b{7=24YirO zjZ92raWwAUFOw1rLWrH)WU2PsAL7-Fd_mFfVUldj7$TlohmoApjnlVkU=6Mn)fhIP zV(yd~x%>UrR=+?&b1!|g#6&HlMWl;2kNbX4k{&J7U*XN7f|Cz0s@muyDHm20=>KIB4BwBSRj9f64!T>z+w@$;m>L5ZS^rPv5ox zOvX%IW+GDHL1701%!tARVve4~SyLC(mRZ`NH%HjXM?*W1$UjZ2R@!VAlFOt{pE;VL z2i*qL_7**X9ZN>gq>Ie(&l9K6l%{F7jaxpNsv@BDN!-A*jkkX9`W01J>p}f6-DUSL zepB5w+6d~5jen*G0#7O4)lNT(NKjfcXFYfUvz3F!fS z3Ltq?X5OF_>PrercUBn7g>i^3L}I%+INCh25@Z96T(g0#9=ra?%Z?r=*3sjL(Jad1 zs!#AGQd!?<*@`X+7$&-mqfCfltRJjBXg8U}auA~j9H#=Ld9`tl2AMn<`FKSc^ukv@ z_zSZIL#`6ZXV18!1Io7iXOu1G0I4l<#_)*mPMBGTSJNB7qG=7G6t%!TNZ9sWc^A7` z@0q#D)&gFLt`>cWpUL*E9chYV6ug*vhBGuUCQ|-r@zQ3NC?852Tj}WPJVJr2F$dN0 zFtaAB4Wt*1VixwT>OIbsrWq?)<{LClvb_xX`v(y0juAh~<`qY=BQ_oBkIlQG7l(rU zTSGA(CZlB^oiK;iy^M#5B-}46iyo=qGx^gzUO#j8W!@Mgu{`z$w301WKV{`dX<|s$ z*a80r-yv@4YHza+v!tAJskmsiF1Wh2>UPEyY0kW z&|VK4SoUk%NNi$hH;FFic7Y~+)w5qSF~cgG+uL*#c=z11^L))aIp(Ff-mTqX{cM#= zWH-|67AXN?jW_5fx6%fx%rw%^blA0ZFY*$b4NI1~?n3E%HhE`gR`{7clYbD4r zX~+A_8TPXBRmv(kOkgp>)ge~ug~+zp4eLTAuf>kN7wT1A{A8-(eek@M_C9Pi5cwvF zQg&glT@NAG#1<=eLKAa~bUfP{^OG?7yXsq@L{{;w@!C;q3~{DVIlz}PYxe3MeZNXs zVbEOW`g1Pxz?afl#>ul=xO2d6QJ+fu*A7~fc_s_2O(+S%4pfpP-Of7izgE;Fs{d`W z;@u*PZLR?i3fThUg$wi{Q^cnBE}hS#0$oLKgW0y29Xn-;2DQ23NzOgI#_+ZComVUA3LIc^aH zIFx3R%Y=pkHcNyZ3YoMII}Qb@1yd6CheEfgDZ?QegQ1YAIvbZa57}y80myLFQZX(I z!y)u=EelcS>#CKZ%q~2@SczvC3f+n^HS6hcQp(`dh(qZnVE%>z4zj)s1#FHtz6}NL zL~rtDKl$B2Tu`tHH5 zcrs@w?KW<*hO!NXrFk_JxIKPF>8ODjZcN#y3}xstl#X6zX*fzNrle(Nkr+;`lgiwW zJRFG5a$zXMX!FfOf`$8_9pp}jp%hMf4(t%^JZi*bb@8a-V7IdZL$O=+b@6Ixub78h zjF_-b&ShOR_|6tFqFKy79j+b5RMk@yCPXq5ir$I7giM(c!4wR#80F$-XPAq7v5pPJ z9ec~-35r^N4^)q2r4%RO$(s#ZD7J9!uF}=4*}|+bOd)?G=V=_lK)ix@2r<1w6>*llJD#%^pk zS-=u_+<+5wkW3J?z(Ydav0Y*&+?y{cmfGSq4Ew2JWO)o*f(^a%H5QOnGfh`ZV40*4 zAw!nT$39#*9~}og+x_fibtI~=Vj|YKTy&vd;W=E+en?Q3n?4nTcA13z688GXj+-M%n^w9IN^oV5kV08gX-hn1gp zpN&v9HnDoWs&R?_n6k?Xde!tiXxC%0`f1;)S9PyOp9vSk_UiO(^zGLT0T=EH_O#l# z^B@eMB?(1~f-Y$jD@=*j?>42DY6|rUMS4NoQ#_2%IOQ=vA4lNQ+9i z8By0;Xi=LT;X-^`n8t92efTYixor7Z$u*&~D;Ac`9`{;>;lS;5l_k(Ju;h_Z&JZ>M zJsriO>UP1U7KCJ*T9FiKu9M^=RStoc@qd!?6U_;d=}0-VYQV;^xFg-%bI zLIp8O3Mf)D6YY>x(DF|q*2u7CQuiVx&;BMdXY#P<7MmRFO#bwK^0S!Ow;hvztIqx< z4>fjXCn`=f_AfNIg7HY4{V+%!OG4jov$*d~ zqtg$W>Q>-k8#7sm)iW{yTVz;SOfL!55?f)j*0>OD^~p-Pxa)kQvp6`n%t>@|WC$Y{ zBl7G4DI$B1f$hLh&{;6ovbz{3yXuvyh{c{ zD0t3lIT*MVy~E7{w=+ST3*zwxlS0CD%xPI0`J6cFhVs&*q{taeZU?-F6Hmx$YtQsx zDr=26j={ja)%oU7UQ~UzRB3~W=-r!zc`q(Yh{So6l$oe8n5Q@VX8qxl5NbPLX%8lM zT)ug@_kCjN6I$95;Hn@e@1_-+VpQNV%xz4GxuW{agITLwALeQ#tzPb8q1&dD7UuJe z4(Q~ge$lx|?N2_+g%*1zWKe;3o*=h2b&@BD$wgTt%nz zYtb{Ii!xb#q8F`Uwauqtw0#vX)1VQp^$xiM`Zf7r< z2T2%R=4ShZ6kCpgsw%rZek3WWCtYYtJ{KZTTR@5X`ZgC~`hG+*KeDuw{c$6nJ5lL?Hnt&Kpds&!7pQ*h&O`yvJi4>a!s1lhc#TxIcDxSa9 zR4gfcE7n6+NzFBNkwo3C-0lSRa;Mqoo71+r%mwqpfbKJsuE^aVG~2DoLIV~Ajegzz zI4Bevf!!7EZDt|8P)GHb#qU7ikq+8YqW;~*w!CYTQAS^|ISb#4A(6$Ugv2C~!dcRc zz--5r-+`t(${X@-=hK6>O7?>%Ci5U8B5})g^!8c`3Fez{iyd=2bL8PhX*=$AY_%jT z@hxsz46faeL+TzFP$`|YfJfc`c1OSMu4)$Xp!GSx@;#7ZnX`ml<;~^pek>bAXAB4T$!JALA;%6rk-OP8J5q@GG8QyT=%jx) zdtTI{pO!9_&Q$gyDR8^V8}b&fj?>*NogWwl*9$dIP@M zuxaGHvAtpK$d%!|(UWtBNPqe0RpE*;#m$B=ve(KwgS5x7$ELsQ$4(@b>N6HjrHoDd zDnzwX9j^(Ok0_2TM$RDoWKygp#U?^p)aHtC9W}fh(C;E4i`4$)k@N7c9a%TBNc;`d z>xOV>q(yEaTtzACR$Xr{et1hIwW#w|xhr^Uk&y3?-N;+5aHI0Fww^UIJ90H}1?^~& z;^gqSkwd(FL%7LuT+4e`g`4Q=>j>RRZ@!P`$$cd=qmeVH+1jvI9TBcOZWiC-sNDYdxRLXy-=^>rYV-?!uUtf4+|%cYLxs>`K=?=`g%kor@uvjzBJcbtB-1+K0cLy^3JWJFUVzN9W4+( zsvj>WW-sN;(zZ?b^miRIGk_b{^Y%{KyGd^=Zsg=!g)p^$-^5o(gq=A+&yjGOFbV`< zB%DC_8sG%NPM~K_!kr7NfLoDYF097A8uuy0jp5Rq9SgSxjt{rBkky192izJsKHQG5 z6Zl_4Nn?~ShU-+qPo=z5DKAihMj=Oz!E(4A@0?1y+XHvtS8oSk1os^YS&Q#Zq(2R~ zv%NRQxQ&I=dF!skA4j^o0cVi#ay} zCjR>a4*=A9)$4(jbbBBGBb4?aN;`|%t_L;@q~D19Y~UQ=T;LSo!C7C7F%ql8LxA4{ z9!hx+qsFVPzH7q6$?FlMSJ{sQHUX#6l9Pb|jF57(z325({k8xai3#9z`bgummDINd zw&C6m?4T!ZV`bbOe7IZKMckuEf4=3lhIx9|FiHFcysh@`0rmo?&~vx4-n)m5)1!HB zAMa^grs&b*0rl(%{{6s(zyaVOps^OEya>3Mo-KP-Uf)`yd8K4 z@4ORu7w~Scewu^ zIE|ToE8+&*`(yl@iF*VyUbIm2`d)zM>pzhGhfsl2tgr7E{xNI}|3vzq0RK$9=G4F7 zPI&oK^7&W7{tfsU?#lt;!OwwTKs`j;l>aYz=ih-};TFF92kHJ3_%ADGEc`cNVI&tu zMws^_tHUTThJO`sD_}Kn9AUQxjt5QvP8?YuZZmRDSTl02(c+Uv#^9SMPe6XR<$1di z;mqoADsi`mK;MD)?udIWa5*Ec9(prgNZO z{yg9z!0%DNhXM~H&BK950BYltXoculf1a-nk0kvj;9$6K*gSGx*fJuzsPTMIn6Q4^ z5VnqJE}k8>(SO@V9vpViKX56`kB88otHUnde-!X8^47XBJm1t;tHb%k?*=C6=QCLe zZVgUdfWO~gdUti$L)c#G{%By|$U~voiRO7*Mfay@i{|Wp(hlOQ=I4ck9RLmjhk%QK zi-E(yV}MJ5#{zlq;c?{iOY+gUBs?t;R|HBx8K{tE8khlQfhz636qv(Z1L{BnXaX(L zUk0>+4$vj62OOk~d7cZvBCtf<5#aH_6M!cIPXe9{Tn;=1cq;HT;OW3Kc;}gv`79fo z2f%wmTLF#1b8KuhZVzMqTOFQD`sV@92VMZY5V#U}5%6N*CBRF0_hrD#fmZ-m5%x;p zRluu(*Ni-zUez_0*7O&?$jn`oRU(LI30Nx0^33xN`7T~SG+kk6; zw*&70-U+-5csKAK;Jv{6fcFC*06s{2K14k}jQbrE;7h;_z?TX83h-6hGzEMO_t$}M0N(^|B+a*gZv)=}ejoS) z;17X60&W8S82A&)`cs~N27DLzbMpEgVc!SlfxiHDlHXqfe+6i~{u=lJ@HeFYA@C#M zZ-Ku9{+@7O!`6o%kE{>>!2ACQ{1fmK;Gc>A7vQI){a4`MMuZ1xzCJSi%j00VJlBLf0jB|X&WSEPBHRV?FB)??W!x3e`Yrk*8uTunqC7zfVG-G$W9;VeD8GUc3U@2#e+ZPa%gZ*QaB z-sT5bytn;J0I9hIg|KRC-u9=`2xcC0DFN)6PDV%5BC)5 z_vcOs7vjD>X)ghUIR{945I6)}1Y8Up1|9=k0z4MT1COIkh1~8?%uR+8?s9HZr~uQz z3@}UnRnlF`a}KEGc7!_52GFE0Tfk+x3+U&xeyC4`L#f?+LOZuNbaIal-Q2#=%T0y( z-2SkTyD%)~4uqxL!EhvZC_FxQQFub`;_$@W;qWB#dvXq$K6goYO75}Ysld~KrvuLb zo=MqfGcwQOc?Ixn;5oo^f#*@z!9Gs?`~29KFW~t?+Ic1LBHVY)<-?1q$8liqOL+IC zu7HSik3g+s5k^oLCD^Y{>6N7(C$KLzPSda|p@ z``)y16#pA|_l>|QCe1*@N%!Vh=IiN$G0Ofi@L}L1z_mzCk}2Q9JI{?}I^I6o@_sAf zZ=;WI1wZkUn9p$C_c4c97wG$&=Y81X-cEn3p2w5dNl06on=j4XQho({=N-8s&#~~% zT#080@5+@e{H3{y$yD#AE*F!}dvepHT1}e<*RdJWPSe-3*0%Rj?)zfh)u->zRjocB zu<%Qf2QQ_BOL=EDd@wf?K1BS7i4z_O2ZRf!(Vrh-JlF7lEqL`2^0^N9DDW}hzg53*oc5I(=RXpUXWk ze4aV~1@ibJX}$#9kduU{xLo)$a4ql^;1GEBRp#s0a!vY3@y_qw7D;l|ke~IE{JxR9 zEQZ5#k>fYX`$phfxptQR+oanT>v#%M=xt&?-y!ec2mS!~L-6m9fSZ7C0)I>%e**j| zaEN#REZ2?s<-&KVr+AJ(=e_Ucda<6X!}l%iJY%7HuMU5K|1Sa2szbnE@!nqpKOpXJ zsM8N~3)#D;QN~Gdvff`y;YYb6;cs(~4}VA6zbB8AkcdyPe2>O zB6Gj^gqu%IqTbBSl&)=f0?@+ zdXUD3b)I~FMSq|lqfMUXBqa6Q-(r1F3;#ZlpVrM^k;i`k{|She{}=GzKp2HT0CK=6 za1iM~;mz16^46$y04_7#YE>yzC7YF z{6u{3p@ct-K7T8k6n{U2hvR<)AbRsio|^#at=^xzD&pZQ!)6;k@#d$rLf(TzvNbmx~xOd6*TuLmzRURQ_1qi+b0A#C;l?r<1JQ zqtS`eLW};pY*aF*xA(`q+m3nul6=3BdlNcO)!T8bLmASAba|&oz2<=hK)WJKly_wG zf#LDw`2^fgB(}8CI&-{PQ<1JYpKcp|7MZQ;1&a;pC z-si*cWx@o>42`0JNYmT=>xJYNR995|gkUO|3W0j~sp%GxLz_bSSHHSikX zwZQ9u*N@&MTs@ljLGgI5JAMOkZybF`c+=>+Xv4e6@m=7n>y}kV&T&ycWLxSd`S>*c zFVE`t?(i1sAvx%+#J>%=26#Jlc?W%XZ_>V#=evM+1Mji&#h1P(ym$0H;eDg;4ey5@ zNE`Wq(f5ZBj(#9~2s-#-;3I(Kplf+vN1j^uKRW92(8uuKm9hFbbr+BM37($>BoBRx z=cfV5Mc4C`9QGNWp9MY#d>;4$@I^rM`b&UxcsG#emw~U)9`*BAd43J}I`9qPo4}30 zw}5ZczV86PPx?Oq{t);h;3o3=W8hNC{1cvk3j7)HUEt3F=`z2^^ZUSG0DlSm75V)& zdHn$R8{mh)j{tF5e@pz|0e?>){22HL;2(j1BJ3x;cRaN4pK-s8zW5i)|LN!l!@m;# zZ@|wf>u03>Iq(bMm%zV|eh8h`hr_Q%9~u6GGX4|zFVb$JzR%CSGW<8RIE+0%jEo&0 za=_>qS`lE?*qU%FU^Q^u7;D_vN8s$l5q|_+(KvfwO6TS4pYSKGfjq|?M^50~6GyLw zU$~ZcueEp2B;ARWaXfX<`?n#@8sHk*CjIV7Fv=&7eF#p^Y2zv6bz5MBdf$%csle@l zI{a3hDXbI^BhAC)2DShbz*b-z zApTDI2sgG@1}eZbdCdT`xU0bTMnB9x zn%Dc%u}_7$u}_B@^*oJw*J(q8G)??1o|ge_paXP)9xx9q0E@s9>5q{1@jRb^|B1kp z@IRUIo=6#&^Lz?%PX(Sv+n!}(@J--G;9G$7qtcDO4LF@K`8HwS0e&C& z1K4 ze-Heacm4tRN8q1;pAh!Xz`p=L1^$(=e*=C7{2cfNVZQ|a9rzXSAHaVC{{{Rv@hB0) z2#^CtfiXb+xoQ=TN_Rlhxg=OY2pzD+zgulfr8hNgCv^I8);#mj8vxeeG3?4XRDz%I&s z6#nypMRq7JWv6oYs?Uc>`tSn!YtO1Lg}vk{Im+n=ygL4Uz!b0_xG>iD0C^n*4gnVd z7Xychdkk<1@K_)Z{ERvIINmGZE&?T>3VzQLUdCMkrhyq?7MK9G099Zsa4GN30pf{j z`0If7TpB!^Knu7GXak*9w+&sOx9XHIziLfbAZ!s>qHafk#{*9Qo=ARA;`wCYa^NYz zQ%UzU;OW3KfM){F0I{sQ2q4A2V+zY=&6aW4j50=yJ>8R=e5 znpXf<5&ufyRk&XbyaxAcdA<&KJ?XCI`3B&Pz?*~XiXRyotDg2)XZa)6Ga6RFl z0X_?SZq?Vr=XrhsD3Z??SA8>liKo**rwgk84a6rp_+{ME`3%!T^jn0h{a;=6E#%{G z(VA}=jd=)bfcP@)VSjDaw{d+3S>=TA_5Y`_vjC6kSR3%1$k{!+n}h^k2=1;8Qlw~+ z;zfdMac^-g6b(=)P)cc`r9g3acM1fz;4Z-lZvXq8$p-Fid!L`@d70TcGv9pUa%N6Q zdo*dN&)at=Z{iOTUgj<$`!Mz+aFpluXobfDPasp*)l&3-$9Z;+`KR=Gcl}N>FVtaA z;C>QL;r}%K&@s|-2IS3h46@F`IXE9Edm&ymo45VJcHFwZxPae_a0xEs?+RSSehsc; zzkzuZZejN_vo`Foa#G-9{q=dE+kp+#9b1PO$~uPph>7n<7!&O9XVFT|o{$D)E~Z_D za?;>SP9EF~JfrRho?@MIT0P*|L(=pJ9%Fw3ao8mvo??Fn&*24bFChkA!E1N}Z+Z3( z-UBb5lmUL=4*?JeCOE)p9#@umQn^gt>4OJ?a1VwMlQ}VjVh_VTF(iSckPMPT3P=fF zNCm0!8%~%skQUNGdef^iU}l6&kQuT-R>%g~AqV7yT!haJc_1(3!!17)fPzrSq+f?m zAOecurzprdp<={e96mMAsuJdT>KJ`Ceo8@U_zcQGSf$x(qMSdF^9b_%x@wD_zV|{>C)**v zJxH4z171$GWXHV&{yIV@=nP$;D|CbI&;ufwWA()R8oq&EumBmok=KX3?F+T&SNie0 zKMa8O_!$U;%(7~*t@{l%#Js77nzz)q=5001yrYJjvFbbgjKKd$7=?W_jDfKbh1_xG zT~5i|BaQb+W3U=eo=@P}_xPWP>C{5k*u7S0%4%Wj45dAh{Zi3CQ z1-8Nr!fYd5+hK=!kN)-F^6UX^=iiAOy5%?{~eD{Pv#SZvDXBgV%u$s-pWY(ul_T!oy}9p^i`$28nr|1do!w&LDl?%DK z(Jv3=1sOYP8Nn)_L&gd7U(zJ;={j_jF-Erq2gU3lO=KGRk{`KADgK(+3t$i7WZ=K| z1&pTn@zPrqbOftHk&gSvEG;x0h;hbWWlZ>2QCvB|d$fvSU4$6a<6OJC_ z$?pnK5#6KduRq6L2`WPss0tftCto1Dnj_c<<5Z*qkr{@J>W(0z2GoRF_^FNmI`Adb zg?dmQ8bBm8ght?1A(WF+jfuAjG=*l+99rlKKN8)UU zeqK&zvK^~B;lDF@Il-8eIJ!7OjIPiPx317oE&tH=>fQ_&THp3R!3fo{i?0}uH3wFaE*bDpMFW3(U;2=c9Avg?2;3yn}<8T5_ z!YMcnXCMa7!Z|n(7vLgXg3E9PuEI6A4maQ?+=AP12V&ta+=Kh@03O04cnnV<4xYj@ zM-qeOoO+I3@_z<(N!p#4Q@$zelfFsqx<4;4%y{8QY`lb5@EYF0TX+ZWK{*o}2Ka$L z1VA8|-~cCB&LoD*NuLkgyPysyqiH3xql=!e%bw+A2BaXT>@}w_8ppc_6E1|t zkqAN|3=%^UND9dyIi!G;5(n4IQbB46hcvjQg>;Y}GT@dGGGWgQSs*K9gX~a{z9k3G zb3!i24SAfwDlg=N{7?W2LLuTT%=1qm0*XLUC ztq7lEuLPB$3RHzJpc+(%8c-8zL2al5UqW4|2lb%=M3RRMk=F>bF~5Tu7eb&3_NLDK zsu}Lhv9|!(k77xvyxFJB-;p$WD)JnHz+vX~_HOBRSKt{|qxg=G~EuS-$j0KK}xex3!E6 zw)|g_I}@g&`z+G*n=>P0I(;U7X2Tya2j)V1@^T(&_|ut9%_qGJU?D658Mn(gGTCFy zVty}yrOwRkbm;lqGJeb4ZMicGBXodT!S9vMtdu2d8tT((Sc97`t84~c3-=af{w;T4 zSmzMG>`ku6+~CZPj@ikF?Bs*2(`8=5o-=Gj#wOV8%w%jK{90!=W1)X$V=M7)Bfjm< z9E|H9-|LY39CBwv-|5izIoxUod9f4ZUdJ77666DRDu~amcHu7f$Mjy8-lvxM>1LwoA%l(Xfn17MR{cr#d@>}+MqcIPWp2JSr%h!8VN1T#ocAD^a41LQQIgR7a zoW=<_Nm-q8<}yyB!x^5-{&fuIS>&99^Vl!&>>~C{a2c-PcGa1ivdB#ua+3yj29R~# zd5P2ns~fh=V09C>TjbeoxI?&DxC{4y`GUId%!3SvdVoCH=X^+6JaXn`9>@C)+@3%j zJjMSrc<#){yoCKK++M;fcnxplIcvDLnD3l&8$B2dAsHlx6d-4=Q(}5C zy%s$eq$cb`Zli=-@~$_+NTedrJK`Vo{`a$HX^C4htvZi>& zxeI;gFTEuvxQmdIBE(gMxTJ3T@>y*oUP?fgq!K1CZux)%`1U#=oKTL-tD^Bb7Q-wK zpMuUG(O2HlS2Rjk`mU(#bC=|KDe$smD7RH5P4dp5G;UgN#(&~317)Ecl!ppdCA-Wj zT9wgL*3hhKXj}6BCX_hB>>EZE@l(dtEycda;8hp>Ez`0Q3y8%s^Le=^tBcy8BDGS zS~ta2>2Vtm|}oq#o%$=Ro~tW!k?sjrexQvW60oyjlB>n?=p3f-VP>F;6HV{gKz1LHIP>e}^9 z<^}pbVo%cZwN;-~=r-~VetJP~Wb}c)$jNUtu-i>P($$~e5ri24x{RbvMA~g$bdqeBbr%_wqy*~^>PgIWnA{v?XkoqdrPu^8D+WEI2dm=NB`!; z*_=2SXko3!)of+8WeE2VzzrswI1;2sZJCyb+@0NbITCzXoyYmsOW?RAP z512#Tb4kNI_|uMyvncGl%*QTsW;xR=d$bE|-I*tnu0^mImcUZnm%(ya0V`n@thQQF zzH(PW^k2@{)yi0dyUYXDVy**r#o1rwoC)>~un{&Pb2Ds#t?(Qjwqb6E9k3HWyI?o$ zfxT91BOA9773jQ_vuskvx?SuejGWE;i@ek&J=ppQQVJ_ zcfLC*$Fa-V!0ojCoyfTc(iTtPeiGzd;3>@0a0X)FES!V$aDjZiXe}X~!RnG#NnJ)C zsmIa}T|s}@2b6PwSMhrdJ+9++18x%T7TmVvyd(2Cs}1QB_pj(D#N<9dvSQ&b_Iq$2 z`vY4(a}Rz$g2(X0YD;ZtOCGgFMzD%QzO=)qc701?JhK+_j$GH#=jd<=UO)@{h`*Qk ze+6Fle&u@#uWdh^4J7}N7g!Wrt^{F05$y_`zQ8ZC1>GX z?dc=s25EbA;7Kab65Hh3D5e8GEBj< zl<{)Bu5R(qSR1)GO9Q$O5?#B~w@NszQz|>Y9{=Fh^B>&g7OsSozD{o9`qI*iK2*yL zM;|%wFZp1wo=t;JdTisIq#al9c)vO=Lpk#$>nE9i>v2xBlevZH+9yGWXv#I69lq~> zxXHZH;omPIFZB6$_H2k(pRetokQW(9b4K*b1etAF1DI!v-d=TxGLms33(vAbHpmV+ zASdL4+>pnY$D~iEYoI+J^yO!`-yrJThy0Z~l-G`HaD2K&U*@aG$&Y&hD2UubP#8Xe z2-lE!8RA#&^)O4f;~i?xo2ZerE9xS+peSLA#p@OB_wE0~&#?c8pW)PBU-|0sqa`IS zc{BM^#%{k- zr{p|{zGo%%tt@iNL3yYEFKK7;-k=73U`5L1bErg{UF$!VIfd*m`s%3eGb$sy3RHzJ zpc+(%WqwiYu}7gx6uLwiHIO57h?+dpb5iD8*rhM8O}uqn-0$RfU2!ANo)B9U^|3zo z2CgV0(lyR#=o)J@a*ZeFc{hN+CeW1U+?!!snm}9=h--q;%#Q0rJR4c7%le`@{^gyT z#F3W%xRUhOE(t4ZLeb5qi@v`k_n2BDzZLRY6Q&J(g`c+24tsm(fZiQlVX6~!hA!wg zg&Xo+T@#IN=39phb9r@-M@{TH4eM5ft;&A z_uC4yFVD2zlZ<}E-5&qtI!vL?s7$@kAm zWlT+YK9uz7e&|Q^=FW#}8ue3u>p}Ky($X&tBTt5telIuu<=aEPeYWp#8v!Gs4DQ*; z?@`E;`(>js$KWpCJs3;)DD)l&<6YAkPpEf1{~pFt_ado-6S1FStgBC(nuLGu`=BFd zf^naWoci{>Vg_xAxgTz_zZ7JDVL;}la@NpyuJBh|{#5+@2-86B3jc&T9ezgE4EV)0 zlk(z>9QK(o3x4DI?=TzwfH|&N23m%^?rxx_&3Gq>j(F_t@s4@1GD&A21&| zk}nGgzYrF|Vvu&W1aqnD59GMjGRkhbYYuY(eV$U*zmoPy5Ls>cy$roq5N0LFy_Z#( zt8E!`?fxtac_;mU6mIkJy9QJ4q^>2OLpT{MdkE{$Wj$;F?$OxcHsVk2PHw{duqVmc zWqxmQ&8PgCTjGBkzqi8<*a^E_3(&o&+U+W)_P7>O77L6jtWWpyY@cgU{BQREh@UZy zJd)>2{>y!7!tedIzh&|MmM6F$u-#WA{61*^UYYPa+Wx&N;kWENdimOeE|WF!=32Y{ z$iAzjNBYM@u66XUymzqoNAwx#!{~klj>0k5TH`pJfRnDml#*_@B{5GC=V{vf8Hhp7 zS^S@a^Kbz!y52E1@vR!#-z9!uhAVItu7QkS@?6G}w|;N!`I>yo<~nk3@cbs`EzH|+ z2YW2$UCeu!_mTYo^C3Kf$MA$_ap2`N25)3I>%e{_?($6xFXs_6*!|3U;~D8V?#iv6 zy9%loE;%!;)Jy!mqI~5%)N5qOx1wZ@^9Fl8`jEFge+Tb@LkRXcJ_D0u1&U*iit7%F zV}Q!U%>hoZK>ARZn|D~)<@}!qGswMxJlFl3o+t5~IZ!bELLd=xBGE~&IXBv6SH{tV z{MulI@;nR@LlQ^|vaU?#-e@FuZ(=-_^%G|gAv1FZx&Q9vs~$f0>FgP&K!=nN>$k-o zzrF6QMiRnRq~4~&J%TTPNS~4#+2N4JEnfkl-@~2`(&HxsWQ0t%{~ffU9aOa)=qlr| z{B!>V%s%KABP;P$M;A$t zKkYpmzeSJiAH^?amBX!N<#g|6PR;%(m-QXy0-I3h`dh+{g_y&4GZ|DO@@Y5GFAN4d3$i1k3{O%6}U?2>F!7v1d z!nZIChQoI-0!G3p7!6}!EOAD`IJexHkUMI;n{<~_6Wm_)Jxp}V+EDJWtyGiT+#Tl2 zX}qQ5Y~dc`2lp;xGHu|H<-6Ck&zR!gXH0eTUJj3@ zLBVRVdz-O@a7$sC`!8d;TkdV|H&zhWN>~M}-6K>MzR(jz`OAH#HMp$>Y4f+xOUi5= zzvVk3>oGULM%V3$C*9+pQ6m383#Wa$MiRGPP?NSqoe6x zqUm1_+WSUt$-^^*3FT`yN?iWu)0FHt8mQ@FMV>Xy6!$|B(}pHGj6z#88@jbw|IV= zuy-Jq-*@32GVWtOfQRr19=nelPaqDS^81-PRy{{XI=-aDsSxyas~5hN? zuiPn(*VL6a{C*4Xz&9^_kDbf0j5GQjvw__Y{2`pLG6i@}8iAfuhUsC32~Lk&Ssu|% zzkhaNcZ0`sntu8;I-EuazWC{pyEdE?K#uf_vR@y9JrRUL7$g=u?nyAyVtV=NQ&LY1 zWhh^Mib2k4BN=j&d(N`zIg8s_yF5}5UrJus1`*B+sp93NM!#_MOXHC*N*%}l@pxT$ z^G=uoy#GjtpHNy6J9}vPoObNf^&IcM-6}odGk8RgBSuETWb!;$nLTnYKzhz@$qu8}8#2zZW`OK|mvO(u=%Vk-vbA^0k{2?#$Ljfr0k*{K9QW@-hrfZDd zVX6@F_4?|%9k=wya!k*U~rWJ=U9qku@Rv_@ucgaTEiQ zSKK3C+PX!)+_LjLOx-j-hb9aR}aWD`NG(P_;_^wGfhDLL+Z7BdF&z2biURgTyf;o1lb#r^pwRe zX{_aW#C+-z{vX-0YLhm3A6O^hIs0Xv$CTY;{5-~wFTAX6({l$}-+`}7en>j&kq7lL z8$e{dzB0ZvL|!A(+}I;;oP2BWION75G7h=EI?9>3z^9}y&S>J%^Sh>mYlf`m5NAE3 zOyyaC689FK=jiYAtM|oQ;x6N#ybEcHzgC_Xc05gRZw)1>d-AQ62)?$aW$ATr8_!nC zl=%jJy3mKUMd#YaEB0<*q2nt%f7+3*_RxWRnn(Yf)^1m<`w7!2K}XrIl<%kPHInmO zbm%NHLFVXPG3A|&#M2F=t=wd;FLy?}^ScN1gstNJc;vKGZ#B^K znt5X>HOM1#j8bZ_=Z!JMlY@KRIn+?hZ}C42hQoI-0!BhdbQpyxYnIXII0nW-6zKh1 z<}l>Jc$k3O_xP3X=1jz##P1(qGEDJ|Qd80AM~|MnPUHDcFr8=NsxNg!bd+zTFh3$L ziT7uo&wyW${VU9bSs?usU+!8 zIA6-s-|m%f?aKRF_7U(u2j;>&koRSOV$O#JpwG4Fw$RqPZl6`?i#$2IztHoJdLw6h zKAa0$gpP|nSBX>VuT@RWSri~oIu{fpoG zVO_lL2QUxvTmA%fG+ZSA4`CjLBOu=^Jn9MbOJ>LAr;g!v9RDXUPr@lU4QC(*&cZo( zPrA<|`vUfhaEUOHw8qQ0UBP}8u3>+|WixrdB>8xq-#2i-=`s9nVcv#25R1Fyt>nuD z|AOi+hxqmRO`VQ=JikxAKJYkcc@!?_yb!q(hPe@89(gR={V^}TU0jfU;tBAJqn_($ zrr%TIea7$S$a?`V@n6?;`Q_uQ1-w8atb9ks%?Mjcy|UYeZyp_>UfcaF_vuKh)Ze$5 z@8CV~Ax33@UyyExt9cjUk39fv05yNNbAW(r6dG#BY}ls8D)9^Y_8m(<{UrxPADk8^3h=hCiA@*PO6YZ~OO zBJ^rl1Jbvo#VsAAhYUgaRYu5!yvz_yR%OAI^kn6EHpmV+g34DbSB47uw}1Z!vPM5{ literal 0 HcmV?d00001 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base (复件).dae b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base (复件).dae new file mode 100644 index 0000000..5bc4e16 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base (复件).dae @@ -0,0 +1,238 @@ + + + + + Stefan Kohlbrecher + Blender 2.61.4 r43829 + + 2012-02-03T14:20:49 + 2012-02-03T14:20:49 + + Z_UP + + + + + + + + 0 0 0 1 + + + 0.1 0.1 0.1 1 + + + 1 0 0.0106101 1 + + + 0.5 0.5 0.5 1 + + + 50 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 1 1 1 1 + + + + 1 1 1 1 + + + + 1 1 1 1 + + + + 1 1 1 1 + + + 50 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0 0 0 1 + + + 0.1 0.1 0.1 1 + + + 0.05496641 0.05496641 0.05496641 1 + + + 0.5 0.5 0.5 1 + + + 50 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0.15 0.15 0.15 1 + + + 0.23 0.23 0.23 1 + + + 0.5 0.5 0.5 1 + + + 1 1 1 1 + + + 20 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + + + + + + + + + + + + + + + 0.8236175 0.673869 -1.366201 -0.8236175 0.6738689 -1.366201 -0.8236175 0.6852639 -1.361481 0.8236175 0.6852641 -1.361481 -0.8236175 0.6899839 -1.350086 0.8236175 0.689984 -1.350086 -0.8236175 0.6852639 -1.338691 0.8236175 0.685264 -1.338691 -0.8236175 0.6738689 -1.333971 0.8236175 0.673869 -1.333971 -0.8236175 0.6624739 -1.338691 0.8236175 0.662474 -1.338691 -0.8236175 0.657754 -1.350086 0.8236175 0.6577541 -1.350086 -0.8236175 0.6624739 -1.361481 0.8236175 0.6624741 -1.361481 0.8236175 0.673869 -1.350086 -0.8236175 0.6738689 -1.350086 0.8236174 -0.6738691 -1.350086 0.8236174 -0.662474 -1.361481 0.8236174 -0.6738691 -1.366201 -0.8236175 -0.6738691 -1.350086 -0.8236175 -0.6738691 -1.366201 -0.8236175 -0.6624742 -1.361481 0.8236174 -0.6577541 -1.350086 -0.8236175 -0.6577541 -1.350086 0.8236174 -0.662474 -1.338691 -0.8236175 -0.662474 -1.338691 0.8236174 -0.6738691 -1.333971 -0.8236175 -0.6738691 -1.333971 0.8236174 -0.6852641 -1.338691 -0.8236175 -0.6852641 -1.338691 0.8236174 -0.689984 -1.350086 -0.8236175 -0.689984 -1.350086 0.8236174 -0.6852641 -1.361481 -0.8236175 -0.6852641 -1.361481 0.5614412 0.08138328 0 0.5614412 0.07047998 -0.04069161 0.5614412 0.04069161 -0.07047998 0.5614412 0 -0.08138328 0.5614412 -0.04069161 -0.07047998 0.5614413 -0.07047998 -0.04069161 0.5614413 -0.08138328 0 0.5614413 -0.07047998 0.04069155 0.5614412 -0.04069167 0.07047992 0.5614412 0 0.08138328 0.5614412 0.04069155 0.07047998 0.5614412 0.07047992 0.04069167 2.084521 0.08138328 0 2.084521 0.07048004 -0.04069155 2.084521 0.04069167 -0.07047986 2.084521 0 -0.08138328 2.084521 -0.04069155 -0.07047998 2.084521 -0.07047986 -0.04069167 2.084521 -0.08138328 0 2.084521 -0.07048004 0.04069155 2.084521 -0.04069173 0.07047992 2.084521 0 0.08138328 2.084521 0.04069155 0.07048004 2.084521 0.07047992 0.04069173 0.5614412 0 0 2.084521 0 0 0.6738689 0.6899839 -1.332763 0.6852639 0.6852639 -1.332763 0.6899839 0.6738689 -1.332763 0.6852639 0.6624739 -1.332763 0.6738689 0.657754 -1.332763 0.6624739 0.6624739 -1.332763 0.657754 0.6738689 -1.332763 0.6624739 0.6852639 -1.332763 0.2994973 0.3156122 -0.303241 0.3108922 0.3108922 -0.303241 0.3156122 0.2994973 -0.303241 0.3108922 0.2881023 -0.303241 0.2994973 0.2833823 -0.303241 0.2881022 0.2881023 -0.303241 0.2833823 0.2994973 -0.303241 0.2881023 0.3108922 -0.303241 0.6738689 0.6738689 -1.332763 0.2994973 0.2994973 -0.303241 0.3156121 -0.2994973 -0.303241 0.6899838 -0.673869 -1.332763 0.6852638 -0.6624739 -1.332763 0.3108922 -0.2881023 -0.303241 0.6738688 -0.657754 -1.332763 0.2994972 -0.2833823 -0.303241 0.6624738 -0.6624739 -1.332763 0.2881022 -0.2881023 -0.303241 0.6577539 -0.673869 -1.332763 0.2833822 -0.2994973 -0.303241 0.6624738 -0.6852639 -1.332763 0.2881022 -0.3108923 -0.303241 0.6738688 -0.6899839 -1.332763 0.2994972 -0.3156122 -0.303241 0.6852638 -0.6852639 -1.332763 0.3108922 -0.3108922 -0.303241 0.2994972 -0.2994973 -0.303241 0.6738688 -0.673869 -1.332763 -0.673869 -0.6738688 -1.332763 -0.685264 -0.6852638 -1.332763 -0.6738691 -0.6899837 -1.332763 -0.2994973 -0.2994972 -0.303241 -0.2994973 -0.3156121 -0.303241 -0.3108923 -0.3108922 -0.303241 -0.689984 -0.6738687 -1.332763 -0.3156123 -0.2994972 -0.303241 -0.685264 -0.6624737 -1.332763 -0.3108923 -0.2881022 -0.303241 -0.673869 -0.6577538 -1.332763 -0.2994973 -0.2833822 -0.303241 -0.662474 -0.6624737 -1.332763 -0.2881023 -0.2881022 -0.303241 -0.6577541 -0.6738688 -1.332763 -0.2833824 -0.2994972 -0.303241 -0.662474 -0.6852638 -1.332763 -0.2881023 -0.3108922 -0.303241 -0.3156121 0.2994973 -0.303241 -0.6899837 0.6738693 -1.332763 -0.6852637 0.662474 -1.332763 -0.3108922 0.2881023 -0.303241 -0.6738687 0.6577541 -1.332763 -0.2994971 0.2833824 -0.303241 -0.6624737 0.662474 -1.332763 -0.2881022 0.2881023 -0.303241 -0.6577537 0.6738691 -1.332763 -0.2833822 0.2994973 -0.303241 -0.6624737 0.6852641 -1.332763 -0.2881022 0.3108924 -0.303241 -0.6738685 0.689984 -1.332763 -0.2994971 0.3156123 -0.303241 -0.6852637 0.6852641 -1.332763 -0.3108922 0.3108923 -0.303241 -0.2994971 0.2994973 -0.303241 -0.6738687 0.6738691 -1.332763 1.969194 0 0.1871857 2.886225 -0.3798463 0.2218515 2.671059 -0.7018645 0.2218514 1.969194 0 0.2620601 2.349041 -0.9170302 0.2218514 1.969195 -0.9925863 0.2218514 1.589348 -0.9170302 0.2218514 1.26733 -0.7018646 0.2218514 1.052164 -0.3798465 0.2218515 0.9766083 -2.36674e-7 0.2218515 1.052164 0.379846 0.2218516 1.26733 0.7018642 0.2218517 1.589348 0.9170299 0.2218517 1.969194 0.992586 0.2218518 2.34904 0.91703 0.2218517 2.671058 0.7018645 0.2218517 2.886224 0.3798464 0.2218516 2.96178 3.25924e-7 0.2218515 0 -0.5614412 0 0.08138316 -0.5614412 0 0.07047986 -0.5614412 -0.04069161 -2.69139e-7 -2.084521 0 0.07047975 -2.084521 -0.04069155 0.08138298 -2.084521 0 0.04069155 -0.5614412 -0.07047998 0.04069143 -2.084521 -0.07047992 0 -0.5614412 -0.08138328 -1.69274e-7 -2.084521 -0.08138328 -0.04069167 -0.5614412 -0.07047998 -0.04069179 -2.084521 -0.07047998 -0.07048004 -0.5614413 -0.04069167 -0.07048016 -2.084521 -0.04069167 -0.08138334 -0.5614413 0 -0.08138352 -2.084521 0 -0.07048004 -0.5614413 0.04069161 -0.07048028 -2.084521 0.04069155 -0.04069173 -0.5614412 0.07047998 -0.04069197 -2.084521 0.07047992 -1.3514e-7 -0.5614412 0.08138328 -3.81294e-7 -2.084521 0.08138328 0.04069143 -0.5614412 0.07047998 0.04069125 -2.084521 0.07048004 0.07047986 -0.5614412 0.04069167 0.07047963 -2.084521 0.04069173 0.379846 -2.886224 0.2218516 -2.61752e-7 -1.969194 0.1871857 0 -2.96178 0.2218515 -3.25223e-7 -1.969194 0.2620601 0.7018641 -2.671058 0.2218517 0.9170297 -2.34904 0.2218517 0.9925858 -1.969194 0.2218517 0.9170296 -1.589348 0.2218517 0.701864 -1.26733 0.2218517 0.3798459 -1.052164 0.2218516 -3.6789e-7 -0.9766083 0.2218515 -0.3798466 -1.052164 0.2218514 -0.7018647 -1.26733 0.2218514 -0.9170304 -1.589348 0.2218513 -0.9925865 -1.969194 0.2218513 -0.9170305 -2.34904 0.2218513 -0.7018648 -2.671059 0.2218514 -0.3798467 -2.886225 0.2218514 -2.96178 4.69959e-7 0.2218515 -2.886225 0.3798471 0.2218514 -1.969194 5.26331e-7 0.1871857 -1.969194 5.89802e-7 0.2620601 -2.671059 0.7018652 0.2218514 -2.34904 0.9170308 0.2218513 -1.969194 0.9925867 0.2218513 -1.589348 0.9170306 0.2218513 -1.26733 0.7018649 0.2218514 -1.052164 0.3798468 0.2218514 -0.9766083 4.99106e-7 0.2218515 -1.052164 -0.3798457 0.2218516 -1.26733 -0.7018638 0.2218517 -1.589348 -0.9170294 0.2218517 -1.969194 -0.9925855 0.2218517 -2.349041 -0.9170294 0.2218517 -2.671058 -0.7018638 0.2218517 -2.886224 -0.3798456 0.2218516 -0.5614412 -0.07047981 0.04069167 -0.5614412 -0.0813831 0 -2.084521 -0.08138269 0 -2.084521 -0.07047933 0.04069173 -0.5614412 -0.04069137 0.07047998 -2.084521 -0.04069095 0.07048004 -0.5614412 2.10574e-7 0.08138328 -2.084521 6.61367e-7 0.08138328 -0.5614412 0.04069179 0.07047998 -2.084521 0.04069226 0.07047992 -0.5614413 0.0704801 0.04069161 -2.084521 0.07048058 0.04069155 -0.5614413 0.0813834 0 -2.084521 0.08138382 0 -0.5614413 0.0704801 -0.04069167 -2.084521 0.07048046 -0.04069167 -0.5614412 0.04069179 -0.07047998 -2.084521 0.04069209 -0.07047998 -0.5614412 1.54427e-7 -0.08138328 -2.084521 4.49347e-7 -0.08138328 -0.5614412 -0.04069149 -0.07047998 -2.084521 -0.04069113 -0.07047992 -0.5614412 -0.07047981 -0.04069161 -2.084521 -0.07047945 -0.04069155 -2.084521 5.49213e-7 0 -0.5614412 1.61804e-7 0 2.37238e-7 0.5614412 0 -0.08138304 0.5614412 0 -0.07047975 0.5614412 -0.04069161 8.29287e-7 2.084521 0 -0.07047921 2.084521 -0.04069155 -0.08138245 2.084521 0 -0.04069137 0.5614412 -0.07047998 -0.04069083 2.084521 -0.07047992 2.29861e-7 0.5614412 -0.08138328 7.29421e-7 2.084521 -0.08138328 0.04069185 0.5614412 -0.07047998 0.04069238 2.084521 -0.07047998 0.07048016 0.5614413 -0.04069167 0.07048076 2.084521 -0.04069167 0.08138352 0.5614413 0 0.08138412 2.084521 0 0.07048022 0.5614413 0.04069161 0.07048088 2.084521 0.04069155 0.04069191 0.5614412 0.07047998 0.04069256 2.084521 0.07047992 2.86009e-7 0.5614412 0.08138328 9.41441e-7 2.084521 0.08138328 -0.04069131 0.5614412 0.07047998 -0.04069072 2.084521 0.07048004 -0.07047975 0.5614412 0.04069167 -0.07047903 2.084521 0.04069173 -0.3798453 2.886224 0.2218516 7.9091e-7 1.969194 0.1871857 8.67901e-7 2.96178 0.2218515 8.54381e-7 1.969194 0.2620601 -0.7018634 2.671058 0.2218517 -0.9170291 2.349041 0.2218517 -0.9925853 1.969195 0.2218517 -0.9170292 1.589348 0.2218517 -0.7018637 1.26733 0.2218517 -0.3798456 1.052164 0.2218516 6.30322e-7 0.9766083 0.2218515 0.3798469 1.052164 0.2218514 0.7018651 1.26733 0.2218514 0.9170309 1.589348 0.2218513 0.992587 1.969194 0.2218513 0.9170311 2.34904 0.2218513 0.7018656 2.671059 0.2218514 0.3798475 2.886225 0.2218514 0.3369342 0.5615574 -0.2994973 0.3369344 -0.5615572 -0.2994973 -0.3369344 -0.5615574 -0.2994973 -0.3369344 0.5615576 -0.2994973 0.3369346 0.5615574 0.2994973 0.3369344 -0.5615576 0.2994973 -0.3369344 -0.5615574 0.2994973 -0.3369344 0.5615572 0.2994973 0.5615572 0.3369344 -0.2994973 -0.5615572 0.3369347 -0.2994973 0.5615576 0.3369343 0.2994973 -0.5615572 0.3369344 0.2994973 0.5615572 -0.3369344 -0.2994973 -0.5615575 -0.3369343 -0.2994973 0.5615574 -0.3369347 0.2994973 -0.5615575 -0.3369344 0.2994973 + + + + + + + + + + 1 0 0 0.6302378 0 -0.7763603 0.6302378 0.548967 -0.548967 -0.6302378 0.548967 -0.548967 -0.6302378 0 -0.7763603 -1 0 0 0.6302378 0.7763603 0 -0.6302378 0.7763603 0 0.6302378 0.548967 0.548967 -0.6302378 0.548967 0.548967 0.6302378 0 0.7763603 -0.6302378 0 0.7763603 0.6302378 -0.548967 0.548967 -0.6302378 -0.548967 0.548967 0.6302378 -0.7763603 0 -0.6302378 -0.7763603 0 0.6302378 -0.548967 -0.548967 -0.6302378 -0.548967 -0.548967 1 0 0 0.6302378 0 -0.7763603 0.6302378 0.548967 -0.548967 -1 0 0 -0.6302378 0.548967 -0.548967 -0.6302378 0 -0.7763603 0.6302378 0.7763603 0 -0.6302378 0.7763603 0 0.6302378 0.548967 0.548967 -0.6302378 0.548967 0.548967 0.6302378 0 0.7763603 -0.6302378 0 0.7763603 0.6302378 -0.548967 0.548967 -0.6302378 -0.548967 0.548967 0.6302378 -0.7763603 0 -0.6302378 -0.7763603 0 0.6302378 -0.548967 -0.548967 -0.6302378 -0.548967 -0.548967 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 -2.53709e-6 1.46479e-6 1 0 0 -1 -1.46479e-6 -3.92489e-7 1 0 0 -1 -1.46479e-6 3.92489e-7 1 0 0 -1 -2.53709e-6 -1.46479e-6 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 0 0 -1 0.06228822 0.8714866 -0.486404 0.6329844 0.6329844 -0.4456618 0 0 1 0.4264656 0.4264656 0.7976318 -0.06045717 0.6646626 0.7446516 0.8714866 0.06228822 -0.486404 0.6646626 -0.06045717 0.7446516 0.6226387 -0.4941862 -0.6066775 0.4941862 -0.6226387 0.6066775 0.06045717 -0.6646626 -0.7446516 -0.06228822 -0.8714866 0.486404 -0.4264656 -0.4264656 -0.7976318 -0.6329844 -0.6329844 0.4456618 -0.6646626 0.06045717 -0.7446516 -0.8714866 -0.06228822 0.486404 -0.4941862 0.6226387 -0.6066775 -0.6226387 0.4941862 0.6066775 0 0 1 0.6646626 0.06045717 0.7446516 0.4941862 0.6226387 0.6066775 0.6226387 0.4941862 -0.6066775 0.8714866 -0.06228822 -0.486404 0 0 -1 -0.06228822 0.8714866 0.486404 0.06045717 0.6646626 -0.7446516 -0.6329844 0.6329844 0.4456618 -0.4264656 0.4264656 -0.7976318 -0.8714866 0.06228822 0.486404 -0.6646626 -0.06045717 -0.7446516 -0.6226387 -0.4941862 0.6066775 -0.4941862 -0.6226387 -0.6066775 -0.06045717 -0.6646626 0.7446516 0.06228822 -0.8714866 -0.486404 0.4264656 -0.4264656 0.7976318 0.6329844 -0.6329844 -0.4456618 0 0 -1 -0.06228822 -0.8714866 -0.486404 -0.6329844 -0.6329844 -0.4456618 0 0 1 -0.4264656 -0.4264656 0.7976318 0.06045717 -0.6646626 0.7446516 -0.8714866 -0.06228822 -0.486404 -0.6646626 0.06045717 0.7446516 -0.6226387 0.4941862 -0.6066775 -0.4941862 0.6226387 0.6066775 -0.06045717 0.6646626 -0.7446516 0.06228822 0.8714866 0.486404 0.4264656 0.4264656 -0.7976318 0.6329844 0.6329844 0.4456618 0.6646626 -0.06045717 -0.7446516 0.8714866 0.06228822 0.486404 0.4941862 -0.6226387 -0.6066775 0.6226387 -0.4941862 0.6066775 0 0 1 -0.6646626 -0.06045717 0.7446516 -0.4941862 -0.6226387 0.6066775 -0.6226387 -0.4941862 -0.6066775 -0.8714866 0.06228822 -0.486404 0 0 -1 0.06228822 -0.8714866 0.486404 -0.06045717 -0.6646626 -0.7446516 0.6329844 -0.6329844 0.4456618 0.4264656 -0.4264656 -0.7976318 0.8714866 -0.06228822 0.486404 0.6646626 0.06045717 -0.7446516 0.6226387 0.4941862 0.6066775 0.4941862 0.6226387 -0.6066775 0.06045717 0.6646626 0.7446516 -0.06228822 0.8714866 -0.486404 -0.4264656 0.4264656 0.7976318 -0.6329844 0.6329844 -0.4456618 0.03490257 -0.006942212 -0.9993667 0.04047429 -0.008050978 0.9991481 0.03431242 -0.02292704 0.9991481 0.02958893 -0.0197705 -0.9993667 0.01977074 -0.02958875 -0.9993666 0.02292686 -0.03431266 0.9991481 0.008050799 -0.04047453 0.9991481 0.00694251 -0.03490239 -0.9993667 -0.00694251 -0.03490239 -0.9993667 -0.008050799 -0.04047447 0.9991481 -0.02292686 -0.03431266 0.9991481 -0.01977074 -0.02958875 -0.9993666 -0.02958893 -0.01977056 -0.9993666 -0.03431248 -0.02292704 0.9991481 -0.04047429 -0.008051037 0.9991481 -0.03490257 -0.006942331 -0.9993666 -0.03490257 0.006942749 -0.9993666 -0.04047429 0.00805062 0.9991481 -0.03431248 0.02292662 0.999148 -0.02958899 0.01977092 -0.9993666 -0.01977074 0.02958917 -0.9993667 -0.02292686 0.03431224 0.9991482 -0.008050799 0.04047405 0.9991481 -0.00694251 0.03490281 -0.9993667 0.00694251 0.03490281 -0.9993667 0.008050799 0.04047411 0.9991481 0.0229268 0.03431224 0.9991482 0.01977074 0.02958923 -0.9993667 0.02958899 0.01977092 -0.9993667 0.03431248 0.02292662 0.9991481 0.04047429 0.00805062 0.9991481 0.03490257 0.006942749 -0.9993667 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 -2.53709e-6 1 1.46479e-6 0 -1 0 -1.46479e-6 1 -3.92489e-7 0 -1 0 -1.46479e-6 1 3.92489e-7 0 -1 0 -2.53709e-6 1 -1.46479e-6 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0.006942689 -0.03490263 -0.9993667 0.00805068 -0.04047423 0.9991481 0.02292662 -0.03431242 0.9991482 0.01977092 -0.02958899 -0.9993667 0.02958923 -0.01977068 -0.9993667 0.03431218 -0.02292686 0.9991481 0.04047405 -0.008050739 0.9991482 0.03490281 -0.00694257 -0.9993666 0.03490281 0.00694257 -0.9993667 0.04047405 0.008050799 0.9991481 0.03431218 0.02292686 0.9991481 0.02958923 0.01977068 -0.9993667 0.01977092 0.02958899 -0.9993666 0.02292662 0.03431242 0.9991481 0.00805062 0.04047423 0.9991481 0.006942689 0.03490263 -0.9993666 -0.006942212 0.03490263 -0.9993666 -0.008051097 0.04047423 0.9991481 -0.02292704 0.03431242 0.9991481 -0.0197705 0.02958893 -0.9993666 -0.02958869 0.01977074 -0.9993666 -0.03431266 0.0229268 0.9991481 -0.04047447 0.008050858 0.9991481 -0.03490233 0.00694251 -0.9993667 -0.03490233 -0.00694251 -0.9993667 -0.04047447 -0.008050858 0.9991481 -0.03431266 -0.0229268 0.9991481 -0.02958875 -0.01977074 -0.9993667 -0.0197705 -0.02958893 -0.9993667 -0.02292704 -0.03431242 0.9991481 -0.008051037 -0.04047423 0.999148 -0.006942152 -0.03490263 -0.9993667 -0.03490263 0.006942152 -0.9993667 -0.04047423 0.008051037 0.9991481 -0.03431242 0.02292704 0.9991481 -0.02958893 0.0197705 -0.9993667 -0.01977068 0.02958875 -0.9993666 -0.02292674 0.03431272 0.9991482 -0.008050858 0.04047447 0.9991481 -0.006942451 0.03490233 -0.9993666 0.00694251 0.03490239 -0.9993667 0.008050858 0.04047447 0.9991481 0.0229268 0.03431266 0.9991481 0.01977074 0.02958869 -0.9993666 0.02958893 0.0197705 -0.9993666 0.03431248 0.02292698 0.9991481 0.04047423 0.008051097 0.9991481 0.03490263 0.006942212 -0.9993666 0.03490263 -0.006942689 -0.9993666 0.04047423 -0.00805062 0.9991481 0.03431242 -0.02292662 0.9991482 0.02958899 -0.01977092 -0.9993667 0.01977068 -0.02958923 -0.9993667 0.02292686 -0.03431218 0.9991481 0.008050799 -0.04047405 0.9991481 0.00694257 -0.03490281 -0.9993667 -0.00694257 -0.03490281 -0.9993667 -0.008050799 -0.04047405 0.9991481 -0.02292686 -0.03431218 0.9991482 -0.01977068 -0.02958917 -0.9993667 -0.02958899 -0.01977092 -0.9993667 -0.03431242 -0.02292662 0.9991482 -0.04047423 -0.00805068 0.9991481 -0.03490263 -0.006942689 -0.9993667 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 2.53709e-6 -1.46479e-6 -1 0 0 1 1.46479e-6 3.92489e-7 -1 0 0 1 1.46479e-6 -3.92489e-7 -1 0 0 1 2.53709e-6 1.46479e-6 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 2.53709e-6 -1 1.46479e-6 0 1 0 1.46479e-6 -1 -3.92489e-7 0 1 0 1.46479e-6 -1 3.92489e-7 0 1 0 2.53709e-6 -1 -1.46479e-6 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 -0.006942689 0.03490263 -0.9993667 -0.00805068 0.04047423 0.9991481 -0.02292662 0.03431242 0.9991482 -0.01977092 0.02958899 -0.9993667 -0.02958917 0.01977074 -0.9993666 -0.03431218 0.02292692 0.9991483 -0.04047405 0.008050799 0.9991482 -0.03490281 0.00694257 -0.9993666 -0.03490281 -0.00694257 -0.9993667 -0.04047405 -0.008050799 0.9991481 -0.03431218 -0.02292686 0.9991481 -0.02958923 -0.01977068 -0.9993666 -0.01977092 -0.02958899 -0.9993667 -0.02292662 -0.03431242 0.9991481 -0.00805062 -0.04047423 0.9991481 -0.006942689 -0.03490263 -0.9993666 0.006942212 -0.03490263 -0.9993666 0.008051097 -0.04047423 0.9991481 0.02292698 -0.03431248 0.9991482 0.0197705 -0.02958899 -0.9993667 0.02958869 -0.01977074 -0.9993667 0.03431266 -0.0229268 0.999148 0.04047447 -0.008050858 0.9991481 0.03490233 -0.00694251 -0.9993667 0.03490239 0.006942451 -0.9993667 0.04047447 0.008050858 0.9991481 0.03431272 0.02292674 0.9991481 0.02958875 0.01977068 -0.9993667 0.0197705 0.02958893 -0.9993667 0.02292704 0.03431242 0.9991481 0.008051037 0.04047423 0.9991481 0.006942152 0.03490263 -0.9993667 -1 3.53806e-7 0 -1 3.53806e-7 0 -0.7071067 -0.707107 -1.40725e-7 -0.7071068 -0.7071068 0 0.7071068 -0.7071069 -4.92539e-7 0.7071067 -0.7071068 -4.8945e-7 1 -3.53806e-7 -1.99016e-7 1 0 -5.97047e-7 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.7071067 0.7071068 3.9156e-7 -0.7071068 0.7071067 4.92539e-7 0.7071067 0.7071068 -5.2772e-7 0.707107 0.7071065 -3.13248e-7 0 0 1 0 0 1 0 0 -1 0 0 -1 -2.65354e-7 1 0 3.53806e-7 1 6.96555e-7 -3.53806e-7 -1 -6.96555e-7 2.65354e-7 -1 0 -0.6557207 -0.6532182 0.3785516 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0 -0.6557207 0.6532182 0.3785516 -0.3785516 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 0 -0.6532182 0.7571337 0 0.6532182 0.7571337 0.3785516 -0.6532182 0.6557207 0.3785821 0.6532182 0.6557207 0.6557207 -0.6532182 0.3785516 0.6557207 0.6532182 0.3785516 0.7571337 -0.6532182 0 0.7571337 0.6532182 0 0.6557207 -0.6532182 -0.3785516 0.6557207 0.6532182 -0.3785516 0.3785516 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0 -0.6532182 -0.7571337 0 0.6532182 -0.7571337 -0.3785516 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.6557207 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 0.6532182 -0.7571337 0 -0.6532182 -0.7571337 0 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 0 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0.3785516 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 -0.3785516 -0.6532182 0.7571337 0 0.6532182 0.7571337 0 -0.6532182 0.6557207 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.3785821 0.6557207 0.6532182 0.3785821 0.6557207 -0.6532182 0 0.7571337 0.6532182 0 0.7571337 -0.6532182 -0.3785516 0.6557207 0.6532182 -0.3785516 0.6557207 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0.3785516 0.6557207 0.6532182 0.3785516 0.7571337 -0.6532182 0 0.7571337 0.6532182 0 0.6557207 -0.6532182 0.3785516 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.6557207 0 0.6532182 0.7571337 0 -0.6532182 0.7571337 -0.3785821 0.6532182 0.6557207 -0.3785821 -0.6532182 0.6557207 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.3785516 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 -0.3785516 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.6557207 0 0.6532182 -0.7571337 0 -0.6532182 -0.7571337 0.3785516 0.6532182 -0.6557207 0.3785516 -0.6532182 -0.6557207 0.6557207 0.6532182 -0.3785516 0.6557207 -0.6532182 -0.3785516 0.6532182 0.7571337 0 -0.6532182 0.7571337 0 -0.6532182 0.6557207 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.3785516 0.6557207 0.6532182 0.3785516 0.6557207 -0.6532182 0 0.7571337 0.6532182 0 0.7571337 -0.6532182 -0.3785821 0.6556902 0.6532182 -0.3785821 0.6557207 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0.3785516 -0.6532182 -0.7571337 0 0.6532182 -0.7571337 0 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 0 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0.3785516 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 -0.3785516 + + + + + + + + + + + + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

60 36 36 36 37 36 61 37 49 37 48 37 60 38 37 38 38 38 61 39 50 39 49 39 60 40 38 40 39 40 61 41 51 41 50 41 60 42 39 42 40 42 61 43 52 43 51 43 60 44 40 44 41 44 61 45 53 45 52 45 60 46 41 46 42 46 61 47 54 47 53 47 60 48 42 48 43 48 61 49 55 49 54 49 60 50 43 50 44 50 61 51 56 51 55 51 60 52 44 52 45 52 61 53 57 53 56 53 60 54 45 54 46 54 61 55 58 55 57 55 60 56 46 56 47 56 61 57 59 57 58 57 47 58 36 58 60 58 61 59 48 59 59 59 48 432 36 433 47 434 48 432 47 434 59 435 46 436 58 437 59 435 46 436 59 435 47 434 45 438 57 439 58 437 45 438 58 437 46 436 44 440 56 441 57 439 44 440 57 439 45 438 43 442 55 443 56 441 43 442 56 441 44 440 42 444 54 445 55 443 42 444 55 443 43 442 41 446 53 447 54 445 41 446 54 445 42 444 40 448 52 449 53 447 40 448 53 447 41 446 39 450 51 451 52 449 39 450 52 449 40 448 38 452 50 453 51 451 38 452 51 451 39 450 37 454 49 455 50 453 37 454 50 453 38 452 36 433 48 432 49 455 36 433 49 455 37 454

+ + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

151 132 135 132 134 132 137 133 135 133 151 133 136 136 138 136 134 136 137 137 138 137 136 137 139 140 140 140 134 140 137 141 140 141 139 141 141 144 142 144 134 144 137 145 142 145 141 145 143 148 144 148 134 148 137 149 144 149 143 149 145 152 146 152 134 152 137 153 146 153 145 153 147 156 148 156 134 156 137 157 148 157 147 157 149 160 150 160 134 160 137 161 150 161 149 161 181 190 178 190 182 190 182 191 178 191 179 191 181 194 183 194 184 194 184 195 183 195 179 195 181 198 185 198 186 198 186 199 185 199 179 199 181 202 187 202 188 202 188 203 187 203 179 203 181 206 189 206 190 206 190 207 189 207 179 207 181 210 191 210 192 210 192 211 191 211 179 211 181 214 193 214 194 214 194 215 193 215 179 215 181 218 195 218 180 218 180 219 195 219 179 219 196 220 197 220 198 220 199 221 197 221 196 221 200 224 201 224 198 224 199 225 201 225 200 225 202 228 203 228 198 228 199 229 203 229 202 229 204 232 205 232 198 232 199 233 205 233 204 233 206 236 207 236 198 236 199 237 207 237 206 237 208 240 209 240 198 240 199 241 209 241 208 241 210 244 211 244 198 244 199 245 211 245 210 245 212 248 213 248 198 248 199 249 213 249 212 249 269 302 266 302 270 302 270 303 266 303 267 303 269 306 271 306 272 306 272 307 271 307 267 307 269 310 273 310 274 310 274 311 273 311 267 311 269 314 275 314 276 314 276 315 275 315 267 315 269 318 277 318 278 318 278 319 277 319 267 319 269 322 279 322 280 322 280 323 279 323 267 323 269 326 281 326 282 326 282 327 281 327 267 327 269 330 283 330 268 330 268 331 283 331 267 331

+ + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

16 0 0 1 3 2 2 3 1 4 17 5 16 0 3 2 5 6 17 5 4 7 2 3 16 0 5 6 7 8 17 5 6 9 4 7 16 0 7 8 9 10 17 5 8 11 6 9 16 0 9 10 11 12 17 5 10 13 8 11 16 0 11 12 13 14 17 5 12 15 10 13 16 0 13 14 15 16 17 5 14 17 12 15 15 16 0 1 16 0 17 5 1 4 14 17 18 18 20 19 19 20 21 21 23 22 22 23 18 18 19 20 24 24 21 21 25 25 23 22 18 18 24 24 26 26 21 21 27 27 25 25 18 18 26 26 28 28 21 21 29 29 27 27 18 18 28 28 30 30 21 21 31 31 29 29 18 18 30 30 32 32 21 21 33 33 31 31 18 18 32 32 34 34 21 21 35 35 33 33 34 34 20 19 18 18 35 35 21 21 22 23 78 60 62 61 63 62 79 63 71 64 70 65 78 60 63 62 64 66 79 63 72 67 71 64 78 60 64 66 65 68 79 63 73 69 72 67 78 60 65 68 66 70 79 63 74 71 73 69 78 60 66 70 67 72 79 63 75 73 74 71 78 60 67 72 68 74 79 63 76 75 75 73 78 60 68 74 69 76 79 63 77 77 76 75 69 76 62 61 78 60 79 63 70 65 77 77 96 78 80 79 83 80 82 81 81 82 97 83 96 78 83 80 85 84 97 83 84 85 82 81 96 78 85 84 87 86 97 83 86 87 84 85 96 78 87 86 89 88 97 83 88 89 86 87 96 78 89 88 91 90 97 83 90 91 88 89 96 78 91 90 93 92 97 83 92 93 90 91 96 78 93 92 95 94 97 83 94 95 92 93 96 78 95 94 80 79 97 83 81 82 94 95 98 96 100 97 99 98 101 99 103 100 102 101 98 96 99 98 104 102 101 99 105 103 103 100 98 96 104 102 106 104 101 99 107 105 105 103 98 96 106 104 108 106 101 99 109 107 107 105 98 96 108 106 110 108 101 99 111 109 109 107 98 96 110 108 112 110 101 99 113 111 111 109 98 96 112 110 114 112 101 99 115 113 113 111 114 112 100 97 98 96 101 99 102 101 115 113 132 114 116 115 119 116 118 117 117 118 133 119 132 114 119 116 121 120 133 119 120 121 118 117 132 114 121 120 123 122 133 119 122 123 120 121 132 114 123 122 125 124 133 119 124 125 122 123 132 114 125 124 127 126 133 119 126 127 124 125 132 114 127 126 129 128 133 119 128 129 126 127 132 114 129 128 131 130 133 119 130 131 128 129 132 114 131 130 116 115 133 119 117 118 130 131 152 164 153 164 154 164 155 165 156 165 157 165 152 166 154 166 158 166 155 167 159 167 156 167 152 168 158 168 160 168 155 169 161 169 159 169 152 170 160 170 162 170 155 171 163 171 161 171 152 172 162 172 164 172 155 173 165 173 163 173 152 174 164 174 166 174 155 175 167 175 165 175 152 176 166 176 168 176 155 177 169 177 167 177 152 178 168 178 170 178 155 179 171 179 169 179 152 180 170 180 172 180 155 181 173 181 171 181 152 182 172 182 174 182 155 183 175 183 173 183 152 184 174 184 176 184 155 185 177 185 175 185 152 186 176 186 153 186 155 187 157 187 177 187 238 252 216 252 217 252 239 253 214 253 215 253 238 254 217 254 219 254 239 255 218 255 214 255 238 256 219 256 221 256 239 257 220 257 218 257 238 258 221 258 223 258 239 259 222 259 220 259 238 260 223 260 225 260 239 261 224 261 222 261 238 262 225 262 227 262 239 263 226 263 224 263 238 264 227 264 229 264 239 265 228 265 226 265 238 266 229 266 231 266 239 267 230 267 228 267 238 268 231 268 233 268 239 269 232 269 230 269 238 270 233 270 235 270 239 271 234 271 232 271 238 272 235 272 237 272 239 273 236 273 234 273 238 274 237 274 216 274 239 275 215 275 236 275 240 276 241 276 242 276 243 277 244 277 245 277 240 278 242 278 246 278 243 279 247 279 244 279 240 280 246 280 248 280 243 281 249 281 247 281 240 282 248 282 250 282 243 283 251 283 249 283 240 284 250 284 252 284 243 285 253 285 251 285 240 286 252 286 254 286 243 287 255 287 253 287 240 288 254 288 256 288 243 289 257 289 255 289 240 290 256 290 258 290 243 291 259 291 257 291 240 292 258 292 260 292 243 293 261 293 259 293 240 294 260 294 262 294 243 295 263 295 261 295 240 296 262 296 264 296 243 297 265 297 263 297 240 298 264 298 241 298 243 299 245 299 265 299 299 332 295 332 297 332 295 333 293 333 297 333 290 334 299 334 297 334 290 335 297 335 286 335 298 336 289 336 285 336 298 337 285 337 296 337 294 338 298 338 296 338 294 339 296 339 292 339 299 340 290 340 298 340 290 341 289 341 298 341 295 342 299 342 298 342 295 343 298 343 294 343 296 344 285 344 286 344 296 345 286 345 297 345 292 346 296 346 297 346 292 347 297 347 293 347 295 348 291 348 293 348 291 349 287 349 293 349 292 350 284 350 288 350 292 351 288 351 294 351 291 352 295 352 288 352 295 353 294 353 288 353 284 354 292 354 293 354 284 355 293 355 287 355 288 356 284 356 291 356 284 357 287 357 291 357 285 358 289 358 290 358 285 359 290 359 286 359 264 360 245 361 241 362 264 360 265 363 245 361 262 364 265 363 264 360 262 364 263 365 265 363 260 366 263 365 262 364 260 366 261 367 263 365 258 368 261 367 260 366 258 368 259 369 261 367 256 370 259 369 258 368 256 370 257 371 259 369 254 372 257 371 256 370 254 372 255 373 257 371 252 374 255 373 254 372 252 374 253 375 255 373 250 376 253 375 252 374 250 376 251 377 253 375 248 378 251 377 250 376 248 378 249 379 251 377 246 380 249 379 248 378 246 380 247 381 249 379 242 382 247 381 246 380 242 382 244 383 247 381 241 362 244 383 242 382 241 362 245 361 244 383 215 384 216 385 237 386 215 384 237 386 236 387 236 387 237 386 235 388 236 387 235 388 234 389 234 389 235 388 233 390 234 389 233 390 232 391 232 391 233 390 231 392 232 391 231 392 230 393 230 393 231 392 229 394 230 393 229 394 228 395 228 395 229 394 227 396 228 395 227 396 226 397 226 397 227 396 225 398 226 397 225 398 224 399 224 399 225 398 223 400 224 399 223 400 222 401 222 401 223 400 221 402 222 401 221 402 220 403 220 403 221 402 219 404 220 403 219 404 218 405 218 405 219 404 217 406 218 405 217 406 214 407 214 407 217 406 216 385 214 407 216 385 215 384 176 408 157 409 153 410 176 408 177 411 157 409 174 412 177 411 176 408 174 412 175 413 177 411 172 414 175 413 174 412 172 414 173 415 175 413 170 416 173 415 172 414 170 416 171 417 173 415 168 418 171 417 170 416 168 418 169 419 171 417 166 420 169 419 168 418 166 420 167 421 169 419 164 422 167 421 166 420 164 422 165 423 167 421 162 424 165 423 164 422 162 424 163 425 165 423 160 426 163 425 162 424 160 426 161 427 163 425 158 428 161 427 160 426 158 428 159 429 161 427 154 430 159 429 158 428 154 430 156 431 159 429 153 410 156 431 154 430 153 410 157 409 156 431 117 118 131 130 130 131 117 118 116 115 131 130 130 131 131 130 128 129 128 129 131 130 129 128 128 129 129 128 126 127 126 127 129 128 127 126 126 127 127 126 124 125 124 125 127 126 125 124 124 125 125 124 122 123 122 123 125 124 123 122 122 123 121 120 120 121 122 123 123 122 121 120 120 121 119 116 118 117 120 121 121 120 119 116 116 115 118 117 119 116 116 115 117 118 118 117 102 101 100 97 114 112 102 101 114 112 115 113 112 110 113 111 115 113 112 110 115 113 114 112 110 108 111 109 113 111 110 108 113 111 112 110 108 106 109 107 110 108 109 107 111 109 110 108 106 104 107 105 108 106 107 105 109 107 108 106 104 102 105 103 106 104 105 103 107 105 106 104 99 98 103 100 104 102 103 100 105 103 104 102 100 97 102 101 103 100 100 97 103 100 99 98 81 82 95 94 94 95 81 82 80 79 95 94 94 95 95 94 92 93 92 93 95 94 93 92 92 93 93 92 90 91 90 91 93 92 91 90 90 91 91 90 88 89 88 89 91 90 89 88 88 89 89 88 86 87 86 87 89 88 87 86 86 87 85 84 84 85 86 87 87 86 85 84 84 85 83 80 82 81 84 85 85 84 83 80 80 79 82 81 83 80 80 79 81 82 82 81 70 65 62 61 69 76 70 65 69 76 77 77 68 74 76 75 77 77 68 74 77 77 69 76 67 72 75 73 76 75 67 72 76 75 68 74 66 70 74 71 67 72 74 71 75 73 67 72 65 68 73 69 66 70 73 69 74 71 66 70 64 66 72 67 65 68 72 67 73 69 65 68 63 62 71 64 64 66 71 64 72 67 64 66 62 61 70 65 71 64 62 61 71 64 63 62 22 23 20 19 34 34 22 23 34 34 35 35 32 32 33 33 35 35 32 32 35 35 34 34 30 30 31 31 33 33 30 30 33 33 32 32 28 28 29 29 31 31 28 28 31 31 30 30 26 26 27 27 29 29 26 26 29 29 28 28 24 24 25 25 27 27 24 24 27 27 26 26 19 20 23 22 25 25 19 20 25 25 24 24 23 22 19 20 20 19 23 22 20 19 22 23 1 4 0 1 15 16 15 16 14 17 1 4 14 17 13 14 12 15 14 17 15 16 13 14 12 15 11 12 10 13 12 15 13 14 11 12 10 13 9 10 8 11 10 13 11 12 9 10 8 11 7 8 6 9 8 11 9 10 7 8 6 9 5 6 4 7 6 9 7 8 5 6 4 7 3 2 2 3 4 7 5 6 3 2 0 1 2 3 3 2 0 1 1 4 2 3

+ + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

137 134 136 134 135 134 135 135 136 135 134 135 137 138 139 138 138 138 138 139 139 139 134 139 137 142 141 142 140 142 140 143 141 143 134 143 137 146 143 146 142 146 142 147 143 147 134 147 137 150 145 150 144 150 144 151 145 151 134 151 137 154 147 154 146 154 146 155 147 155 134 155 137 158 149 158 148 158 148 159 149 159 134 159 137 162 151 162 150 162 150 163 151 163 134 163 178 188 180 188 179 188 181 189 180 189 178 189 183 192 182 192 179 192 181 193 182 193 183 193 185 196 184 196 179 196 181 197 184 197 185 197 187 200 186 200 179 200 181 201 186 201 187 201 189 204 188 204 179 204 181 205 188 205 189 205 191 208 190 208 179 208 181 209 190 209 191 209 193 212 192 212 179 212 181 213 192 213 193 213 195 216 194 216 179 216 181 217 194 217 195 217 199 222 200 222 197 222 197 223 200 223 198 223 199 226 202 226 201 226 201 227 202 227 198 227 199 230 204 230 203 230 203 231 204 231 198 231 199 234 206 234 205 234 205 235 206 235 198 235 199 238 208 238 207 238 207 239 208 239 198 239 199 242 210 242 209 242 209 243 210 243 198 243 199 246 212 246 211 246 211 247 212 247 198 247 199 250 196 250 213 250 213 251 196 251 198 251 266 300 268 300 267 300 269 301 268 301 266 301 271 304 270 304 267 304 269 305 270 305 271 305 273 308 272 308 267 308 269 309 272 309 273 309 275 312 274 312 267 312 269 313 274 313 275 313 277 316 276 316 267 316 269 317 276 317 277 317 279 320 278 320 267 320 269 321 278 321 279 321 281 324 280 324 267 324 269 325 280 325 281 325 283 328 282 328 267 328 269 329 282 329 283 329

+ + + 1 + + + + + + 0 0 0 + 0 0 1 0 + 0 1 0 0 + 1 0 0 0 + 0.1335572 0.1335572 0.1335572 + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.dae b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.dae new file mode 100644 index 0000000..7a96249 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.dae @@ -0,0 +1,229 @@ + + + + + Blender User + Blender 2.79.0 + + 2018-10-19T14:54:03 + 2018-10-19T14:54:03 + + Z_UP + + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 1 0 0.0106101 1 + + + 0.5 0.5 0.5 1 + + + 50 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0.2 0.2 0.2 1 + + + 0 0 0 1 + + + 0.4575223 0.4575223 0.4575223 1 + + + 1 1 1 1 + + + 33 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.05496641 0.05496641 0.05496641 1 + + + 0.5 0.5 0.5 1 + + + 50 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.5925679 0.5925679 0.5925679 1 + + + 0.5 0.5 0.5 1 + + + 20 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + + + + + + + + + + + + + + + 0.8236175 0.673869 -1.366201 -0.8236175 0.6738689 -1.366201 -0.8236175 0.6852639 -1.361481 0.8236175 0.6852641 -1.361481 -0.8236175 0.6899839 -1.350086 0.8236175 0.689984 -1.350086 -0.8236175 0.6852639 -1.338691 0.8236175 0.685264 -1.338691 -0.8236175 0.6738689 -1.333971 0.8236175 0.673869 -1.333971 -0.8236175 0.6624739 -1.338691 0.8236175 0.662474 -1.338691 -0.8236175 0.657754 -1.350086 0.8236175 0.6577541 -1.350086 -0.8236175 0.6624739 -1.361481 0.8236175 0.6624741 -1.361481 0.8236175 0.673869 -1.350086 -0.8236175 0.6738689 -1.350086 0.8236174 -0.6738691 -1.350086 0.8236174 -0.662474 -1.361481 0.8236174 -0.6738691 -1.366201 -0.8236175 -0.6738691 -1.350086 -0.8236175 -0.6738691 -1.366201 -0.8236175 -0.6624742 -1.361481 0.8236174 -0.6577541 -1.350086 -0.8236175 -0.6577541 -1.350086 0.8236174 -0.662474 -1.338691 -0.8236175 -0.662474 -1.338691 0.8236174 -0.6738691 -1.333971 -0.8236175 -0.6738691 -1.333971 0.8236174 -0.6852641 -1.338691 -0.8236175 -0.6852641 -1.338691 0.8236174 -0.689984 -1.350086 -0.8236175 -0.689984 -1.350086 0.8236174 -0.6852641 -1.361481 -0.8236175 -0.6852641 -1.361481 0.5614412 0.08138328 0 0.5614412 0.07047998 -0.04069161 0.5614412 0.04069161 -0.07047998 0.5614412 0 -0.08138328 0.5614412 -0.04069161 -0.07047998 0.5614413 -0.07047998 -0.04069161 0.5614413 -0.08138328 0 0.5614413 -0.07047998 0.04069155 0.5614412 -0.04069167 0.07047992 0.560442 -0.001217126 0.08256208 0.560442 0.03947442 0.07165879 0.5614412 0.07047992 0.04069167 2.084521 0.08138328 0 2.084521 0.07048004 -0.04069155 2.084521 0.04069167 -0.07047986 2.084521 0 -0.08138328 2.084521 -0.04069155 -0.07047998 2.084521 -0.07047986 -0.04069167 2.084521 -0.08138328 0 2.084521 -0.07048004 0.04069155 2.084521 -0.04069173 0.07047992 2.084521 0 0.08138328 2.083522 0.03947442 0.07165884 2.084521 0.07047992 0.04069173 0.5614412 0 0 2.084521 0 0 0.6738689 0.6899839 -1.332763 0.6852639 0.6852639 -1.332763 0.6899839 0.6738689 -1.332763 0.6852639 0.6624739 -1.332763 0.6738689 0.657754 -1.332763 0.6624739 0.6624739 -1.332763 0.657754 0.6738689 -1.332763 0.6624739 0.6852639 -1.332763 0.2994973 0.3156122 -0.303241 0.3108922 0.3108922 -0.303241 0.3156122 0.2994973 -0.303241 0.3108922 0.2881023 -0.303241 0.2994973 0.2833823 -0.303241 0.2881022 0.2881023 -0.303241 0.2833823 0.2994973 -0.303241 0.2881023 0.3108922 -0.303241 0.6738689 0.6738689 -1.332763 0.2994973 0.2994973 -0.303241 0.3156121 -0.2994973 -0.303241 0.6899838 -0.673869 -1.332763 0.6852638 -0.6624739 -1.332763 0.3108922 -0.2881023 -0.303241 0.6738688 -0.657754 -1.332763 0.2994972 -0.2833823 -0.303241 0.6624738 -0.6624739 -1.332763 0.2881022 -0.2881023 -0.303241 0.6577539 -0.673869 -1.332763 0.2833822 -0.2994973 -0.303241 0.6624738 -0.6852639 -1.332763 0.2881022 -0.3108923 -0.303241 0.6738688 -0.6899839 -1.332763 0.2994972 -0.3156122 -0.303241 0.6852638 -0.6852639 -1.332763 0.3108922 -0.3108922 -0.303241 0.2994972 -0.2994973 -0.303241 0.6738688 -0.673869 -1.332763 -0.673869 -0.6738688 -1.332763 -0.685264 -0.6852638 -1.332763 -0.6738691 -0.6899837 -1.332763 -0.2994973 -0.2994972 -0.303241 -0.2994973 -0.3156121 -0.303241 -0.3108923 -0.3108922 -0.303241 -0.689984 -0.6738687 -1.332763 -0.3156123 -0.2994972 -0.303241 -0.685264 -0.6624737 -1.332763 -0.3108923 -0.2881022 -0.303241 -0.673869 -0.6577538 -1.332763 -0.2994973 -0.2833822 -0.303241 -0.662474 -0.6624737 -1.332763 -0.2881023 -0.2881022 -0.303241 -0.6577541 -0.6738688 -1.332763 -0.2833824 -0.2994972 -0.303241 -0.662474 -0.6852638 -1.332763 -0.2881023 -0.3108922 -0.303241 -0.3156121 0.2994973 -0.303241 -0.6899837 0.6738693 -1.332763 -0.6852637 0.662474 -1.332763 -0.3108922 0.2881023 -0.303241 -0.6738687 0.6577541 -1.332763 -0.2994971 0.2833824 -0.303241 -0.6624737 0.662474 -1.332763 -0.2881022 0.2881023 -0.303241 -0.6577537 0.6738691 -1.332763 -0.2833822 0.2994973 -0.303241 -0.6624737 0.6852641 -1.332763 -0.2881022 0.3108924 -0.303241 -0.6738685 0.689984 -1.332763 -0.2994971 0.3156123 -0.303241 -0.6852637 0.6852641 -1.332763 -0.3108922 0.3108923 -0.303241 -0.2994971 0.2994973 -0.303241 -0.6738687 0.6738691 -1.332763 1.969194 0 0.1871857 2.886225 -0.3798463 0.2218515 2.671059 -0.7018645 0.2218514 1.969194 0 0.2620601 2.349041 -0.9170302 0.2218514 1.969195 -0.9925863 0.2218514 1.589348 -0.9170302 0.2218514 1.26733 -0.7018646 0.2218514 1.052164 -0.3798465 0.2218515 0.9766083 -2.36674e-7 0.2218515 1.052164 0.379846 0.2218516 1.26733 0.7018642 0.2218517 1.589348 0.9170299 0.2218517 1.969194 0.992586 0.2218518 2.34904 0.91703 0.2218517 2.671058 0.7018645 0.2218517 2.886224 0.3798464 0.2218516 2.96178 3.25924e-7 0.2218515 0 -0.5614412 0 0.08138316 -0.5614412 0 0.07047986 -0.5614412 -0.04069161 -2.69139e-7 -2.084521 0 0.07047975 -2.084521 -0.04069155 0.08138298 -2.084521 0 0.04069155 -0.5614412 -0.07047998 0.04069143 -2.084521 -0.07047992 0 -0.5614412 -0.08138328 -1.69274e-7 -2.084521 -0.08138328 -0.04069167 -0.5614412 -0.07047998 -0.04069179 -2.084521 -0.07047998 -0.07048004 -0.5614413 -0.04069167 -0.07048016 -2.084521 -0.04069167 -0.08138334 -0.5614413 0 -0.08138352 -2.084521 0 -0.07048004 -0.5614413 0.04069161 -0.07048028 -2.084521 0.04069155 -0.04069173 -0.5614412 0.07047998 -0.04069197 -2.084521 0.07047992 -1.3514e-7 -0.5614412 0.08138328 -3.81294e-7 -2.084521 0.08138328 0.04069143 -0.5614412 0.07047998 0.04069125 -2.084521 0.07048004 0.07047986 -0.5614412 0.04069167 0.07047963 -2.084521 0.04069173 0.379846 -2.886224 0.2218516 -2.61752e-7 -1.969194 0.1871857 0 -2.96178 0.2218515 -3.25223e-7 -1.969194 0.2620601 0.7018641 -2.671058 0.2218517 0.9170297 -2.34904 0.2218517 0.9925858 -1.969194 0.2218517 0.9170296 -1.589348 0.2218517 0.701864 -1.26733 0.2218517 0.3798459 -1.052164 0.2218516 -3.6789e-7 -0.9766083 0.2218515 -0.3798466 -1.052164 0.2218514 -0.7018647 -1.26733 0.2218514 -0.9170304 -1.589348 0.2218513 -0.9925865 -1.969194 0.2218513 -0.9170305 -2.34904 0.2218513 -0.7018648 -2.671059 0.2218514 -0.3798467 -2.886225 0.2218514 -2.96178 4.69959e-7 0.2218515 -2.886225 0.3798471 0.2218514 -1.969194 5.26331e-7 0.1871857 -1.969194 5.89802e-7 0.2620601 -2.671059 0.7018652 0.2218514 -2.34904 0.9170308 0.2218513 -1.969194 0.9925867 0.2218513 -1.589348 0.9170306 0.2218513 -1.26733 0.7018649 0.2218514 -1.052164 0.3798468 0.2218514 -0.9766083 4.99106e-7 0.2218515 -1.052164 -0.3798457 0.2218516 -1.26733 -0.7018638 0.2218517 -1.589348 -0.9170294 0.2218517 -1.969194 -0.9925855 0.2218517 -2.349041 -0.9170294 0.2218517 -2.671058 -0.7018638 0.2218517 -2.886224 -0.3798456 0.2218516 -0.5614412 -0.07047981 0.04069167 -0.5614412 -0.0813831 0 -2.084521 -0.08138269 0 -2.084521 -0.07047933 0.04069173 -0.5614412 -0.04069137 0.07047998 -2.084521 -0.04069095 0.07048004 -0.5614412 2.10574e-7 0.08138328 -2.084521 6.61367e-7 0.08138328 -0.5614412 0.04069179 0.07047998 -2.084521 0.04069226 0.07047992 -0.5614413 0.0704801 0.04069161 -2.084521 0.07048058 0.04069155 -0.5614413 0.0813834 0 -2.084521 0.08138382 0 -0.5614413 0.0704801 -0.04069167 -2.084521 0.07048046 -0.04069167 -0.5614412 0.04069179 -0.07047998 -2.084521 0.04069209 -0.07047998 -0.5614412 1.54427e-7 -0.08138328 -2.084521 4.49347e-7 -0.08138328 -0.5614412 -0.04069149 -0.07047998 -2.084521 -0.04069113 -0.07047992 -0.5614412 -0.07047981 -0.04069161 -2.084521 -0.07047945 -0.04069155 -2.084521 5.49213e-7 0 -0.5614412 1.61804e-7 0 2.37238e-7 0.5614412 0 -0.08138304 0.5614412 0 -0.07047975 0.5614412 -0.04069161 8.29287e-7 2.084521 0 -0.07047921 2.084521 -0.04069155 -0.08138245 2.084521 0 -0.04069137 0.5614412 -0.07047998 -0.04069083 2.084521 -0.07047992 2.29861e-7 0.5614412 -0.08138328 7.29421e-7 2.084521 -0.08138328 0.04069185 0.5614412 -0.07047998 0.04069238 2.084521 -0.07047998 0.07048016 0.5614413 -0.04069167 0.07048076 2.084521 -0.04069167 0.08138352 0.5614413 0 0.08138412 2.084521 0 0.07048022 0.5614413 0.04069161 0.07048088 2.084521 0.04069155 0.04069191 0.5614412 0.07047998 0.04069256 2.084521 0.07047992 2.86009e-7 0.5614412 0.08138328 9.41441e-7 2.084521 0.08138328 -0.04069131 0.5614412 0.07047998 -0.04069072 2.084521 0.07048004 -0.07047975 0.5614412 0.04069167 -0.07047903 2.084521 0.04069173 -0.3798453 2.886224 0.2218516 7.9091e-7 1.969194 0.1871857 8.67901e-7 2.96178 0.2218515 8.54381e-7 1.969194 0.2620601 -0.7018634 2.671058 0.2218517 -0.9170291 2.349041 0.2218517 -0.9925853 1.969195 0.2218517 -0.9170292 1.589348 0.2218517 -0.7018637 1.26733 0.2218517 -0.3798456 1.052164 0.2218516 6.30322e-7 0.9766083 0.2218515 0.3798469 1.052164 0.2218514 0.7018651 1.26733 0.2218514 0.9170309 1.589348 0.2218513 0.992587 1.969194 0.2218513 0.9170311 2.34904 0.2218513 0.7018656 2.671059 0.2218514 0.3798475 2.886225 0.2218514 0.3369342 0.5615574 -0.2994973 0.3369344 -0.5615572 -0.2994973 -0.3369344 -0.5615574 -0.2994973 -0.3369344 0.5615576 -0.2994973 0.3369346 0.5615574 0.2994973 0.3369344 -0.5615576 0.2994973 -0.3369344 -0.5615574 0.2994973 -0.3369344 0.5615572 0.2994973 0.5615572 0.3369344 -0.2994973 -0.5615572 0.3369347 -0.2994973 0.5615576 0.3369343 0.2994973 -0.5615572 0.3369344 0.2994973 0.5615572 -0.3369344 -0.2994973 -0.5615575 -0.3369343 -0.2994973 0.5615574 -0.3369347 0.2994973 -0.5615575 -0.3369344 0.2994973 + + + + + + + + + + 1 0 0 0.6302583 0 -0.7763856 0.6302602 0.5489864 -0.5489864 -0.6302602 0.5489864 -0.5489864 -0.6302583 0 -0.7763856 -1 0 0 0.6302583 0.7763856 0 -0.6302583 0.7763856 0 0.6302602 0.5489864 0.5489864 -0.6302602 0.5489864 0.5489864 0.6302583 0 0.7763856 -0.6302583 0 0.7763856 0.6302602 -0.5489864 0.5489864 -0.6302602 -0.5489864 0.5489864 0.6302583 -0.7763856 0 -0.6302583 -0.7763856 0 0.6302602 -0.5489864 -0.5489864 -0.6302602 -0.5489864 -0.5489864 1 -2.24983e-6 0 -1 2.81229e-7 0 1 -3.37475e-6 0 1 7.8744e-6 0 -1 -3.37475e-6 1.46479e-6 1 -4.49967e-6 0 -1 -1.12492e-6 -3.92489e-7 -1 -1.12492e-6 3.92489e-7 -1 -2.24983e-6 -1.46479e-6 -0.9996916 -0.02150607 -0.01241582 1 2.24983e-6 0 -0.9999209 -0.003253757 -0.01214957 0.9996798 0.02530705 0 -0.9997215 0.01180022 -0.02044135 0.9997214 -0.01180779 0.02044135 -1 5.52928e-7 0 0 0 -1 0.06228959 0.8715052 -0.4864144 0.6329994 0.6329994 -0.4456723 0 0 1 0.4264737 0.4264737 0.7976468 -0.06045901 0.6646835 0.744675 0.8715052 0.06228959 -0.4864144 0.6646835 -0.06045901 0.744675 0.6226522 -0.494197 -0.6066907 0.494197 -0.6226522 0.6066907 0.06045901 -0.6646835 -0.744675 -0.06228959 -0.8715052 0.4864144 -0.4264737 -0.4264737 -0.7976468 -0.6329994 -0.6329994 0.4456723 -0.6646835 0.06045901 -0.744675 -0.8715052 -0.06228959 0.4864144 -0.494197 0.6226522 -0.6066907 -0.6226522 0.494197 0.6066907 0.6646835 0.06045901 0.744675 0.494197 0.6226522 0.6066907 0.6226522 0.494197 -0.6066907 0.871518 -0.06229048 -0.486391 -0.06228959 0.8715052 0.4864144 0.06045901 0.6646835 -0.744675 -0.6329994 0.6329994 0.4456723 -0.4264737 0.4264737 -0.7976468 -0.8715052 0.06228959 0.4864144 -0.6646835 -0.06045901 -0.744675 -0.6226522 -0.494197 0.6066907 -0.494197 -0.6226522 -0.6066907 -0.06045901 -0.6646835 0.744675 0.06228959 -0.8715052 -0.4864144 0.4264737 -0.4264737 0.7976468 0.6329994 -0.6329994 -0.4456723 -0.06229048 -0.871518 -0.486391 -0.6329994 -0.6329994 -0.4456723 -0.4264737 -0.4264737 0.7976468 0.06045901 -0.6646835 0.744675 -0.871518 -0.06229048 -0.486391 -0.6646835 0.06045901 0.744675 -0.6226522 0.494197 -0.6066907 -0.494197 0.6226522 0.6066907 -0.06045901 0.6646835 -0.744675 0.06228959 0.8715052 0.4864144 0.4264737 0.4264737 -0.7976468 0.6329994 0.6329994 0.4456723 0.6646835 -0.06045901 -0.744675 0.8715052 0.06228959 0.4864144 0.494197 -0.6226522 -0.6066907 0.6226522 -0.494197 0.6066907 -0.6646835 -0.06045901 0.744675 -0.494197 -0.6226522 0.6066907 -0.6226522 -0.494197 -0.6066907 -0.8715052 0.06228959 -0.4864144 0.06228959 -0.8715052 0.4864144 -0.06045901 -0.6646835 -0.744675 0.6329994 -0.6329994 0.4456723 0.4264737 -0.4264737 -0.7976468 0.871518 -0.06229048 0.486391 0.6646835 0.06045901 -0.744675 0.6226522 0.494197 0.6066907 0.494197 0.6226522 -0.6066907 0.06045901 0.6646835 0.744675 -0.06228959 0.8715052 -0.4864144 -0.4264737 0.4264737 0.7976468 -0.6329994 0.6329994 -0.4456723 0.03490263 -0.006942212 -0.9993667 0.04047435 -0.008050978 0.9991483 0.03431254 -0.0229271 0.9991481 0.02958899 -0.01977056 -0.9993667 0.01977074 -0.02958875 -0.9993667 0.0229268 -0.0343126 0.9991483 0.008050799 -0.04047453 0.9991482 0.00694257 -0.03490233 -0.9993667 -0.00694257 -0.03490239 -0.9993667 -0.008050799 -0.04047447 0.9991481 -0.02292686 -0.03431266 0.9991482 -0.01977074 -0.02958875 -0.9993667 -0.02958893 -0.01977056 -0.9993667 -0.03431242 -0.02292704 0.9991483 -0.04047429 -0.008051037 0.9991483 -0.03490263 -0.006942272 -0.9993666 -0.03490257 0.006942689 -0.9993666 -0.04047429 0.00805062 0.9991483 -0.03431254 0.02292662 0.9991483 -0.02958899 0.01977092 -0.9993666 -0.01977074 0.02958923 -0.9993667 -0.0229268 0.03431224 0.9991482 -0.008050799 0.04047405 0.9991483 -0.00694257 0.03490281 -0.9993667 0.00694257 0.03490275 -0.9993667 0.008050799 0.04047405 0.9991483 0.02292686 0.0343123 0.9991482 0.01977068 0.02958923 -0.9993667 0.02958899 0.01977097 -0.9993666 0.03431248 0.02292656 0.9991482 0.04047429 0.00805068 0.9991483 0.03490263 0.006942689 -0.9993667 0 1 2.81229e-7 0 -1 4.49966e-6 0 1 1.12492e-6 0 -1 -2.24983e-6 0 1 0 0 -1 0 -2.53709e-6 1 0 0 -1 2.24983e-6 -1.46479e-6 1 -1.12492e-6 0 -1 2.24983e-6 -1.46479e-6 1 5.62457e-7 0 -1 -4.49966e-6 -2.53709e-6 1 0 0 1 1.12491e-6 0 -1 4.49966e-6 0 1 -1.12492e-6 0 -1 -4.49966e-6 0 1 -1.12491e-6 0 1 2.81229e-7 0 -1 -2.24983e-6 0.006942749 -0.03490263 -0.9993666 0.00805068 -0.04047423 0.9991482 0.02292668 -0.03431242 0.9991481 0.01977086 -0.02958899 -0.9993667 0.02958923 -0.01977074 -0.9993667 0.03431218 -0.02292686 0.9991483 0.04047405 -0.008050799 0.9991483 0.03490281 -0.00694257 -0.9993666 0.03490281 0.00694257 -0.9993666 0.04047405 0.008050799 0.9991483 0.03431218 0.02292686 0.9991483 0.02958923 0.01977068 -0.9993667 0.01977092 0.02958899 -0.9993666 0.02292668 0.03431242 0.9991483 0.00805062 0.04047423 0.9991482 0.006942689 0.03490263 -0.9993667 -0.006942272 0.03490263 -0.9993667 -0.008051097 0.04047423 0.9991482 -0.02292704 0.03431242 0.9991482 -0.0197705 0.02958893 -0.9993667 -0.02958875 0.01977074 -0.9993667 -0.03431266 0.0229268 0.9991482 -0.04047447 0.008050858 0.9991481 -0.03490233 0.006942451 -0.9993667 -0.03490233 -0.00694251 -0.9993667 -0.04047453 -0.008050858 0.9991481 -0.03431266 -0.0229268 0.9991482 -0.02958869 -0.01977074 -0.9993667 -0.01977056 -0.02958893 -0.9993667 -0.02292704 -0.03431242 0.9991482 -0.008051037 -0.04047423 0.9991482 -0.006942212 -0.03490263 -0.9993667 -0.03490257 0.006942093 -0.9993667 -0.04047429 0.008051097 0.9991483 -0.03431242 0.02292704 0.9991482 -0.02958899 0.01977056 -0.9993667 -0.01977068 0.02958875 -0.9993667 -0.0229268 0.03431272 0.9991481 -0.008050799 0.04047447 0.9991482 -0.00694251 0.03490233 -0.9993667 0.00694251 0.03490233 -0.9993667 0.008050799 0.04047453 0.9991481 0.0229268 0.03431266 0.9991481 0.01977074 0.02958875 -0.9993667 0.02958899 0.0197705 -0.9993667 0.03431242 0.02292698 0.9991482 0.04047423 0.008051156 0.9991482 0.03490263 0.006942212 -0.9993667 0.03490263 -0.006942689 -0.9993667 0.04047423 -0.00805062 0.9991482 0.03431242 -0.02292668 0.9991482 0.02958899 -0.01977092 -0.9993667 0.01977068 -0.02958917 -0.9993666 0.02292686 -0.03431218 0.9991483 0.008050739 -0.04047411 0.9991483 0.00694257 -0.03490281 -0.9993667 -0.00694257 -0.03490275 -0.9993667 -0.008050739 -0.04047411 0.9991483 -0.02292692 -0.03431218 0.9991482 -0.01977068 -0.02958923 -0.9993667 -0.02958893 -0.01977092 -0.9993666 -0.03431242 -0.02292656 0.9991483 -0.04047423 -0.008050739 0.9991482 -0.03490263 -0.006942689 -0.9993667 -1 5.62457e-6 0 -1 -2.24983e-6 0 1 2.24983e-6 -1.46479e-6 1 2.24983e-6 3.92489e-7 1 2.24983e-6 -3.92489e-7 -1 4.49966e-6 0 1 2.24983e-6 1.46479e-6 -1 -7.8744e-6 0 -1 2.24983e-6 0 0 -1 2.81229e-7 0 1 4.49966e-6 0 1 2.24983e-6 0 -1 -1.12492e-6 2.53709e-6 -1 2.24983e-6 1.46479e-6 -1 -1.12492e-6 0 1 -6.74949e-6 1.46479e-6 -1 5.62457e-7 0 1 4.49966e-6 2.53709e-6 -1 -1.12492e-6 0 1 2.24983e-6 0 -1 1.12491e-6 0 1 4.49966e-6 0 -1 -1.12492e-6 0 1 -4.49966e-6 0 1 -4.49966e-6 0 -1 2.81229e-7 0 1 -2.24983e-6 -0.006942749 0.03490263 -0.9993667 -0.00805068 0.04047423 0.9991482 -0.02292662 0.03431242 0.9991482 -0.01977092 0.02958899 -0.9993666 -0.02958917 0.01977074 -0.9993667 -0.03431218 0.02292692 0.9991481 -0.04047405 0.008050799 0.9991482 -0.03490281 0.00694257 -0.9993666 -0.03490281 -0.00694257 -0.9993666 -0.04047405 -0.008050799 0.9991483 -0.03431212 -0.02292686 0.9991482 -0.02958923 -0.01977068 -0.9993667 -0.01977092 -0.02958899 -0.9993666 -0.02292668 -0.03431242 0.9991482 -0.00805062 -0.04047423 0.9991482 -0.006942689 -0.03490263 -0.9993667 0.006942272 -0.03490263 -0.9993667 0.008051037 -0.04047423 0.9991482 0.02292704 -0.03431248 0.9991481 0.0197705 -0.02958899 -0.9993667 0.02958869 -0.01977074 -0.9993667 0.03431266 -0.0229268 0.9991482 0.04047447 -0.008050858 0.9991481 0.03490239 -0.00694251 -0.9993667 0.03490233 0.006942451 -0.9993667 0.04047453 0.008050858 0.9991481 0.03431272 0.02292674 0.9991481 0.02958875 0.01977068 -0.9993667 0.01977056 0.02958893 -0.9993667 0.02292704 0.03431242 0.9991482 0.008051037 0.04047423 0.9991482 0.006942212 0.03490257 -0.9993667 -1 4.42999e-7 0 -1 2.95333e-7 0 -0.7071066 -0.7071071 0 -0.707107 -0.7071067 0 0.7071068 -0.7071068 -4.69872e-7 0.7071068 -0.7071068 -5.48184e-7 1 -4.42999e-7 -1.99016e-7 1 0 -5.97047e-7 0 0 1 0 0 -1 0 0 -1 -0.7071068 0.7071068 3.9156e-7 -0.7071069 0.7071067 5.48184e-7 0.7071068 0.7071068 -5.24176e-7 0.7071069 0.7071067 -2.34936e-7 0 0 -1 0 0 -1 -2.65354e-7 1 0 3.53806e-7 1 5.90666e-7 -3.53806e-7 -1 -5.90666e-7 2.65354e-7 -1 0 -0.6557323 -0.6532297 0.3785583 -0.7571544 0.653236 0 -0.7571544 -0.653236 0 -0.6557323 0.6532297 0.3785583 -0.3785582 -0.6532296 0.6557322 -0.3785658 0.6532427 0.6557148 0 -0.653236 0.7571544 0 0.653236 0.7571544 0.3785582 -0.6532296 0.6557322 0.3785582 0.6532296 0.6557322 0.6557323 -0.6532297 0.3785583 0.6557247 0.6532222 0.3785844 0.7571544 -0.653236 0 0.7571544 0.653236 0 0.6557323 -0.6532297 -0.3785583 0.6557072 0.6532351 -0.378592 0.3785582 -0.6532296 -0.6557322 0.3785582 0.6532296 -0.6557322 0 -0.653236 -0.7571544 0 0.653236 -0.7571544 -0.3785582 -0.6532296 -0.6557322 -0.3785582 0.6532296 -0.6557322 -0.6557323 -0.6532297 -0.3785583 -0.6557323 0.6532297 -0.3785583 0.653236 -0.7571544 0 -0.653236 -0.7571544 0 -0.6532297 -0.6557323 -0.3785583 0.6532297 -0.6557323 -0.3785583 -0.6532296 -0.3785582 -0.6557322 0.6532296 -0.3785582 -0.6557322 -0.653236 0 -0.7571544 0.653236 0 -0.7571544 -0.6532296 0.3785582 -0.6557322 0.6532296 0.3785582 -0.6557322 -0.6532297 0.6557323 -0.3785583 0.6532297 0.6557323 -0.3785583 -0.653236 0.7571544 0 0.653236 0.7571544 0 -0.6532297 0.6557323 0.3785583 0.6532297 0.6557323 0.3785583 -0.6532296 0.3785582 0.6557322 0.6532296 0.3785582 0.6557322 -0.653236 0 0.7571544 0.653236 0 0.7571544 -0.6532296 -0.3785582 0.6557322 0.6532296 -0.3785582 0.6557322 -0.6532297 -0.6557323 0.3785583 0.6532297 -0.6557323 0.3785583 0.6557323 0.6532297 0.3785583 -0.3785582 0.6532296 0.6557322 0.6557323 0.6532297 -0.3785583 -0.6503974 0.6608045 0.3745942 0.6558382 0.6504973 0.383053 -0.6598159 0.3809048 0.6477304 0.6418532 0.3770365 0.6677336 -0.6564947 -0.020051 0.7540641 0.658137 -8.85044e-4 0.7528977 -0.6533265 -0.3956215 0.645483 + + + + + + + + + + + + + + +

60 5 36 5 37 5 60 5 37 5 38 5 60 19 38 19 39 19 60 19 39 19 40 19 60 22 40 22 41 22 60 24 41 24 42 24 60 25 42 25 43 25 60 26 43 26 44 26 60 27 44 27 45 27 60 29 45 29 46 29 60 31 46 31 47 31 47 33 36 33 60 33

+ + + + +

151 100 135 100 134 100 137 101 135 101 151 101 136 104 138 104 134 104 137 105 138 105 136 105 139 108 140 108 134 108 137 109 140 109 139 109 141 112 142 112 134 112 137 113 142 113 141 113 143 116 144 116 134 116 137 117 144 117 143 117 145 120 146 120 134 120 137 121 146 121 145 121 147 124 148 124 134 124 137 125 148 125 147 125 149 128 150 128 134 128 137 129 150 129 149 129 181 154 178 154 182 154 182 155 178 155 179 155 181 158 183 158 184 158 184 159 183 159 179 159 181 162 185 162 186 162 186 163 185 163 179 163 181 166 187 166 188 166 188 167 187 167 179 167 181 170 189 170 190 170 190 171 189 171 179 171 181 174 191 174 192 174 192 175 191 175 179 175 181 178 193 178 194 178 194 179 193 179 179 179 181 182 195 182 180 182 180 183 195 183 179 183 196 184 197 184 198 184 199 185 197 185 196 185 200 188 201 188 198 188 199 189 201 189 200 189 202 192 203 192 198 192 199 193 203 193 202 193 204 196 205 196 198 196 199 197 205 197 204 197 206 200 207 200 198 200 199 201 207 201 206 201 208 204 209 204 198 204 199 205 209 205 208 205 210 208 211 208 198 208 199 209 211 209 210 209 212 212 213 212 198 212 199 213 213 213 212 213 269 245 266 245 270 245 270 246 266 246 267 246 269 249 271 249 272 249 272 250 271 250 267 250 269 253 273 253 274 253 274 254 273 254 267 254 269 257 275 257 276 257 276 258 275 258 267 258 269 261 277 261 278 261 278 262 277 262 267 262 269 265 279 265 280 265 280 266 279 266 267 266 269 269 281 269 282 269 282 270 281 270 267 270 269 273 283 273 268 273 268 274 283 274 267 274

+ + + + +

16 0 0 1 3 2 2 3 1 4 17 5 16 0 3 2 5 6 17 5 4 7 2 3 16 0 5 6 7 8 17 5 6 9 4 7 16 0 7 8 9 10 17 5 8 11 6 9 16 0 9 10 11 12 17 5 10 13 8 11 16 0 11 12 13 14 17 5 12 15 10 13 16 0 13 14 15 16 17 5 14 17 12 15 15 16 0 1 16 0 17 5 1 4 14 17 18 0 20 1 19 2 21 5 23 3 22 4 18 0 19 2 24 6 21 5 25 7 23 3 18 0 24 6 26 8 21 5 27 9 25 7 18 0 26 8 28 10 21 5 29 11 27 9 18 0 28 10 30 12 21 5 31 13 29 11 18 0 30 12 32 14 21 5 33 15 31 13 18 0 32 14 34 16 21 5 35 17 33 15 34 16 20 1 18 0 35 17 21 5 22 4 61 0 49 0 48 0 61 18 50 18 49 18 61 20 51 20 50 20 61 21 52 21 51 21 61 23 53 23 52 23 61 0 54 0 53 0 61 0 55 0 54 0 61 0 56 0 55 0 61 28 57 28 56 28 61 30 58 30 57 30 61 32 59 32 58 32 61 0 48 0 59 0 78 34 62 35 63 36 79 37 71 38 70 39 78 34 63 36 64 40 79 37 72 41 71 38 78 34 64 40 65 42 79 37 73 43 72 41 78 34 65 42 66 44 79 37 74 45 73 43 78 34 66 44 67 46 79 37 75 47 74 45 78 34 67 46 68 48 79 37 76 49 75 47 78 34 68 48 69 50 79 37 77 51 76 49 69 50 62 35 78 34 79 37 70 39 77 51 96 37 80 52 83 53 82 54 81 55 97 34 96 37 83 53 85 56 97 34 84 57 82 54 96 37 85 56 87 58 97 34 86 59 84 57 96 37 87 58 89 60 97 34 88 61 86 59 96 37 89 60 91 62 97 34 90 63 88 61 96 37 91 62 93 64 97 34 92 65 90 63 96 37 93 64 95 66 97 34 94 67 92 65 96 37 95 66 80 52 97 34 81 55 94 67 98 34 100 68 99 69 101 37 103 70 102 71 98 34 99 69 104 72 101 37 105 73 103 70 98 34 104 72 106 74 101 37 107 75 105 73 98 34 106 74 108 76 101 37 109 77 107 75 98 34 108 76 110 78 101 37 111 79 109 77 98 34 110 78 112 80 101 37 113 81 111 79 98 34 112 80 114 82 101 37 115 83 113 81 114 82 100 68 98 34 101 37 102 71 115 83 132 37 116 84 119 85 118 86 117 87 133 34 132 37 119 85 121 88 133 34 120 89 118 86 132 37 121 88 123 90 133 34 122 91 120 89 132 37 123 90 125 92 133 34 124 93 122 91 132 37 125 92 127 94 133 34 126 95 124 93 132 37 127 94 129 96 133 34 128 97 126 95 132 37 129 96 131 98 133 34 130 99 128 97 132 37 131 98 116 84 133 34 117 87 130 99 152 132 153 132 154 132 155 133 156 133 157 133 152 134 154 134 158 134 155 135 159 135 156 135 152 136 158 136 160 136 155 137 161 137 159 137 152 136 160 136 162 136 155 137 163 137 161 137 152 138 162 138 164 138 155 139 165 139 163 139 152 140 164 140 166 140 155 141 167 141 165 141 152 142 166 142 168 142 155 143 169 143 167 143 152 144 168 144 170 144 155 135 171 135 169 135 152 145 170 145 172 145 155 146 173 146 171 146 152 147 172 147 174 147 155 148 175 148 173 148 152 149 174 149 176 149 155 137 177 137 175 137 152 150 176 150 153 150 155 151 157 151 177 151 238 5 216 5 217 5 239 0 214 0 215 0 238 5 217 5 219 5 239 0 218 0 214 0 238 216 219 216 221 216 239 0 220 0 218 0 238 217 221 217 223 217 239 0 222 0 220 0 238 5 223 5 225 5 239 218 224 218 222 218 238 5 225 5 227 5 239 219 226 219 224 219 238 5 227 5 229 5 239 220 228 220 226 220 238 221 229 221 231 221 239 222 230 222 228 222 238 223 231 223 233 223 239 0 232 0 230 0 238 224 233 224 235 224 239 0 234 0 232 0 238 5 235 5 237 5 239 0 236 0 234 0 238 5 237 5 216 5 239 0 215 0 236 0 240 225 241 225 242 225 243 226 244 226 245 226 240 137 242 137 246 137 243 227 247 227 244 227 240 137 246 137 248 137 243 136 249 136 247 136 240 228 248 228 250 228 243 136 251 136 249 136 240 229 250 229 252 229 243 136 253 136 251 136 240 230 252 230 254 230 243 231 255 231 253 231 240 232 254 232 256 232 243 233 257 233 255 233 240 234 256 234 258 234 243 235 259 235 257 235 240 236 258 236 260 236 243 237 261 237 259 237 240 238 260 238 262 238 243 239 263 239 261 239 240 137 262 137 264 137 243 240 265 240 263 240 240 241 264 241 241 241 243 242 245 242 265 242 299 275 295 275 297 275 295 276 293 276 297 276 290 277 299 277 297 277 290 278 297 278 286 278 298 279 289 279 285 279 298 280 285 280 296 280 294 281 298 281 296 281 294 282 296 282 292 282 299 37 290 37 298 37 290 37 289 37 298 37 295 283 299 283 298 283 295 37 298 37 294 37 296 284 285 284 286 284 296 34 286 34 297 34 292 285 296 285 297 285 292 34 297 34 293 34 295 286 291 286 293 286 291 287 287 287 293 287 292 288 284 288 288 288 292 289 288 289 294 289 291 37 295 37 288 37 295 37 294 37 288 37 284 290 292 290 293 290 284 291 293 291 287 291 288 292 284 292 291 292 284 293 287 293 291 293 285 294 289 294 290 294 285 295 290 295 286 295 264 296 245 297 241 298 264 296 265 299 245 297 262 300 265 299 264 296 262 300 263 301 265 299 260 302 263 301 262 300 260 302 261 303 263 301 258 304 261 303 260 302 258 304 259 305 261 303 256 306 259 305 258 304 256 306 257 307 259 305 254 308 257 307 256 306 254 308 255 309 257 307 252 310 255 309 254 308 252 310 253 311 255 309 250 312 253 311 252 310 250 312 251 313 253 311 248 314 251 313 250 312 248 314 249 315 251 313 246 316 249 315 248 314 246 316 247 317 249 315 242 318 247 317 246 316 242 318 244 319 247 317 241 298 244 319 242 318 241 298 245 297 244 319 215 320 216 321 237 322 215 320 237 322 236 323 236 323 237 322 235 324 236 323 235 324 234 325 234 325 235 324 233 326 234 325 233 326 232 327 232 327 233 326 231 328 232 327 231 328 230 329 230 329 231 328 229 330 230 329 229 330 228 331 228 331 229 330 227 332 228 331 227 332 226 333 226 333 227 332 225 334 226 333 225 334 224 335 224 335 225 334 223 336 224 335 223 336 222 337 222 337 223 336 221 338 222 337 221 338 220 339 220 339 221 338 219 340 220 339 219 340 218 341 218 341 219 340 217 342 218 341 217 342 214 343 214 343 217 342 216 321 214 343 216 321 215 320 176 344 157 308 153 309 176 344 177 306 157 308 174 305 177 306 176 344 174 305 175 304 177 306 172 303 175 304 174 305 172 303 173 302 175 304 170 345 173 302 172 303 170 345 171 300 173 302 168 299 171 300 170 345 168 299 169 296 171 300 166 297 169 296 168 299 166 297 167 298 169 296 164 319 167 298 166 297 164 319 165 318 167 298 162 317 165 318 164 319 162 317 163 316 165 318 160 315 163 316 162 317 160 315 161 314 163 316 158 313 161 314 160 315 158 313 159 312 161 314 154 346 159 312 158 313 154 346 156 310 159 312 153 309 156 310 154 346 153 309 157 308 156 310 117 87 131 98 130 99 117 87 116 84 131 98 130 99 131 98 128 97 128 97 131 98 129 96 128 97 129 96 126 95 126 95 129 96 127 94 126 95 127 94 124 93 124 93 127 94 125 92 124 93 125 92 122 91 122 91 125 92 123 90 122 91 121 88 120 89 122 91 123 90 121 88 120 89 119 85 118 86 120 89 121 88 119 85 116 84 118 86 119 85 116 84 117 87 118 86 102 71 100 68 114 82 102 71 114 82 115 83 112 80 113 81 115 83 112 80 115 83 114 82 110 78 111 79 113 81 110 78 113 81 112 80 108 76 109 77 110 78 109 77 111 79 110 78 106 74 107 75 108 76 107 75 109 77 108 76 104 72 105 73 106 74 105 73 107 75 106 74 99 69 103 70 104 72 103 70 105 73 104 72 100 68 102 71 103 70 100 68 103 70 99 69 81 55 95 66 94 67 81 55 80 52 95 66 94 67 95 66 92 65 92 65 95 66 93 64 92 65 93 64 90 63 90 63 93 64 91 62 90 63 91 62 88 61 88 61 91 62 89 60 88 61 89 60 86 59 86 59 89 60 87 58 86 59 85 56 84 57 86 59 87 58 85 56 84 57 83 53 82 54 84 57 85 56 83 53 80 52 82 54 83 53 80 52 81 55 82 54 70 39 62 35 69 50 70 39 69 50 77 51 68 48 76 49 77 51 68 48 77 51 69 50 67 46 75 47 76 49 67 46 76 49 68 48 66 44 74 45 67 46 74 45 75 47 67 46 65 42 73 43 66 44 73 43 74 45 66 44 64 40 72 41 65 42 72 41 73 43 65 42 63 36 71 38 64 40 71 38 72 41 64 40 62 35 70 39 71 38 62 35 71 38 63 36 48 333 36 332 47 347 48 333 47 347 59 348 46 349 58 350 59 348 46 349 59 348 47 347 45 351 57 352 58 350 45 351 58 350 46 349 44 353 56 341 57 352 44 353 57 352 45 351 43 342 55 343 56 341 43 342 56 341 44 353 42 321 54 320 55 343 42 321 55 343 43 342 41 322 53 323 54 320 41 322 54 320 42 321 40 324 52 325 53 323 40 324 53 323 41 322 39 326 51 327 52 325 39 326 52 325 40 324 38 328 50 329 51 327 38 328 51 327 39 326 37 330 49 331 50 329 37 330 50 329 38 328 36 332 48 333 49 331 36 332 49 331 37 330 22 4 20 1 34 16 22 4 34 16 35 17 32 14 33 15 35 17 32 14 35 17 34 16 30 12 31 13 33 15 30 12 33 15 32 14 28 10 29 11 31 13 28 10 31 13 30 12 26 8 27 9 29 11 26 8 29 11 28 10 24 6 25 7 27 9 24 6 27 9 26 8 19 2 23 3 25 7 19 2 25 7 24 6 23 3 19 2 20 1 23 3 20 1 22 4 1 4 0 1 15 16 15 16 14 17 1 4 14 17 13 14 12 15 14 17 15 16 13 14 12 15 11 12 10 13 12 15 13 14 11 12 10 13 9 10 8 11 10 13 11 12 9 10 8 11 7 8 6 9 8 11 9 10 7 8 6 9 5 6 4 7 6 9 7 8 5 6 4 7 3 2 2 3 4 7 5 6 3 2 0 1 2 3 3 2 0 1 1 4 2 3

+ + + + +

137 102 136 102 135 102 135 103 136 103 134 103 137 106 139 106 138 106 138 107 139 107 134 107 137 110 141 110 140 110 140 111 141 111 134 111 137 114 143 114 142 114 142 115 143 115 134 115 137 118 145 118 144 118 144 119 145 119 134 119 137 122 147 122 146 122 146 123 147 123 134 123 137 126 149 126 148 126 148 127 149 127 134 127 137 130 151 130 150 130 150 131 151 131 134 131 178 152 180 152 179 152 181 153 180 153 178 153 183 156 182 156 179 156 181 157 182 157 183 157 185 160 184 160 179 160 181 161 184 161 185 161 187 164 186 164 179 164 181 165 186 165 187 165 189 168 188 168 179 168 181 169 188 169 189 169 191 172 190 172 179 172 181 173 190 173 191 173 193 176 192 176 179 176 181 177 192 177 193 177 195 180 194 180 179 180 181 181 194 181 195 181 199 186 200 186 197 186 197 187 200 187 198 187 199 190 202 190 201 190 201 191 202 191 198 191 199 194 204 194 203 194 203 195 204 195 198 195 199 198 206 198 205 198 205 199 206 199 198 199 199 202 208 202 207 202 207 203 208 203 198 203 199 206 210 206 209 206 209 207 210 207 198 207 199 210 212 210 211 210 211 211 212 211 198 211 199 214 196 214 213 214 213 215 196 215 198 215 266 243 268 243 267 243 269 244 268 244 266 244 271 247 270 247 267 247 269 248 270 248 271 248 273 251 272 251 267 251 269 252 272 252 273 252 275 255 274 255 267 255 269 256 274 256 275 256 277 259 276 259 267 259 269 260 276 260 277 260 279 263 278 263 267 263 269 264 278 264 279 264 281 267 280 267 267 267 269 268 280 268 281 268 283 271 282 271 267 271 269 272 282 272 283 272

+ + + 1 + + + + + + + 0.1335572 0 0 0 0 0.1335572 0 0 0 0 0.1335572 0 0 0 0 1 + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl new file mode 100644 index 0000000000000000000000000000000000000000..d553efc0bacfc97db3dbfc64c42c526c7b4c92e3 GIT binary patch literal 27084 zcmbWA4Xhkh6~_q(EkZ<0OAJVYKt&10+Q&!fd%KmG0*YYT3Rt9|SP&yn#V_z1ghCWS zlqd+ALX-xLTHg~y`gW@llLm+gL0dk9UqQ5dM$|L}Jpb7_`+jF;&+OJFFI?uH-}#?& z@146dcW3S{x!|L#&;P`ktM)tV{MF~~cg8tqp111E^LP0F|GjR<$o}-0Kee7&dVA}@ z?=S3n`Mp~f73D3fH?&?mwy0YpI)Aj9y{*QG&QnIO=n+MEo_VSfLBst@@4a_S7uq$V z?@JmdnU(yi?AkRVXt-Z#Y_@3MI`@crR6#?2WktPa4fiVzwc)d?4j-)5Qd&vF^_uf=ztXtu$*B?gVf8m`x@;eMsDt3^0?Tq$1fXZiIp z=6ZDv_a(n-D<8W)rfWp#bBR$O=ppgyIv&Rm$#J&`PFfoR|>sIR5fJfdAMFAsv7QB$5-4jU5tfDR5fJfedBtO zsA{-h&tv$wl3yQJu2 z=vQU6(_5@ZBT?1(v{}90taRMCNazz^e>W{TysQ7}%2MS&*wvr@RF1?eHy_Z_-+TTt zuk+w*4sJbUe=6(P!|M(y%8{T3{YuNVBS8)M9HbWsYS6FrcD5_0<~)?c#P$0Yu~RSC zpkHb0imX{fIV{>`pF)Frxd#0zhv9jQTMnKF^>PjRRSv`ZLpgYVP%qb@U+E1WSIWW1 z74@P9eJO4qyDAUQLpk_(rC!w7)vR{gd_YGd{2?2OUe?HOUB7Q}JQ5Gsh_h?wjz+_Y zHp1=7yC`q9sGcs%k)Q_s>iGCB`wY;aFO}EWRk8G_oomo<*@HtgUSYlFIZ-dypx+~H z#CW?!f_YqGqtDxidXZpT(XaF#e$Bxnuyv_HzqY2)d2-+xMU6+z=ODcp6*cHrdOMY8b0nzoh~;o(IU7(f z64any9shE921kM#Dj!kK2GolLHRxA&kHhoOjOKYzFV~=7Thr)|g-9@u3CqFzgL;u* z9`q}{;p3`pdHA@ZUeus3j$LX;f_bRy;rn8oE4MZ2h9ppkA&)zv_RR9ftIW`q}fKUampE9LJGh9vr*Wi^Q)kyQ{tb z3ya21yyN+aeOCUqeaYSL?wnxdXV$H0KfUJOx7>5jJ(YX4XRYSH^LCqj)2Vy3NYGQ% zAo0bUZ)-2!wz2chie4n>mqZ>Ft=@m*+V&40*w#5_5-tuLkoVRwd1a=s22%p(C<>qes~_AG%IN+ zTk7Q+^lPiB)3#RKv31dg{g5{rd#q(Ijrw{!+iI0rNyGYil6tuY{eHI0 zW30BmOO3T<9$MF>UampE@1N<9%C!$QZZ7*n>$=p-HRyNiuGjmxYS7s8_EU;;MXU1E z%QfhCkD2qMLF4{8#di{2`@Ud;dbtMuo>89X_4RhPRjWMDPqT=qmut{3=aooMgYyUV zB5|S3vo_0h^%;I~d)}nYa@~`x{PTHxP1!8htzYjRb>*Tdn@PI|+4U|7dWsq(XtmHj z*KM}$o@RQHU=4{ps+;U{@AMnjcJ&!fIwu$v33`fAk)YLD`&?IBNjnm(A(2OQqvi3^ zsVfR!q;rB%k)Wp-6$x4$ZvFA=Wp5tPAFLsfNA++psDMm$tR&Tfa5cS79YU7GEB=V?!YWIIH z+qha{zDVZ;qas01c^f7spd zfc{_&i9D)r*toL0`eIy>&Iv|Ef}Zj`XvJAO609MSN2Sk;1$V6((`SbAS!?IL&K1pv z<1c(`tp53-k#O;wZW+_(2MKzLQISylzsu$+ofD)Z!5R{IRJ5|Ym+q}L4{MC&1fwEB zPcbSI@_UPYZb*Z4Bv?ZtkBV0M9GTj#JgS687^7kycK6#oy6g|qkzfspJP%rF{{7~5 zjVluL6r*Au`n*{3hO3HkOga**A(7`nD}5fGYv+BrUNXY@L4uxQRLsLxhq_}nPwAS9 zbR<|qBF}?X`W(5z=HYU^WQ06O&{K?xd8qt7``nNQ=}54KM4kt&Y&CK_^B_S_F)G#g z45#bc1!aGbjs$B+#yPrUTIWAdZ&);SBN zwc?@jT)S)49a7ZL%D~HOe_5=FsC=?n>26EqH`G=o+%GlUuQV>6Gr2)`dd(W!%t}vb zQ7;nJJlwC2zkYpZto|HXHILh_c}~wK78l8lmH^7m2Ed`_=I)tOc|Z5{atDGPBaE3iTpU)o{N$eyc^O z6`e>_H7+qLtqw`U^&(N#aKAeK+g?jgM}itEf2g#QhU-P5s^NZh{NBgEq;WMwiNDUBJ!Z8n9o@&;-peQSve)y@l|0US@ToD2pe^F%g}rBk zmF=B2)9HA9eU}>a`=a%Q^wiSwHuH}fA2ciZRj*NR##$w6(C^g`?NsDfA6L|P;?Vnw zoMc74T!Vf;*P9)N!o+Dfwl^&-KxqTiJ^ z3rVBCzDtc;&8qC@N$TYq^m}!gM}2*l8qbw^Xm+Mvu0g+Fo#~H>NKoVGvOn}GL%m3_ zt>|~-9tZlkYS7qs$61G}t@Jraywm}Ws;sSh^&2+-dCacL+V%VM zzwGwHm|gj`b$?ERo}vZ`S{-87y>@lkuCMR1hD09Kx9xu81NQx)wi`~MLH*#2MKzLQIVk4J8a!# zqxDC9eU~*P@~D1c>pXgTKfksLX%s z-(?hpd-d;gC{(26Zm)9AM%`vB2aT9@rZt9+EO6-kdq&`K@h za)MSWtBrJiJVf8ob7qzg67iP+>T2yM?npNH(j7pJs|2Jy%t>`x=7&~*yGp`W+ zK481W%&AVA`9-2?<^7QpR@0v|R5qfcxl4rgO;L^-6{2qiqB7cfKfo*f{N8{B?*~Z7 zsCZvPf}V1M_cf#=!TU23)XoXspOKCP?=wkIJ12ObNjeg|UnW8AoZ$U3=}7Rtp9HmY zg7^KTBf<3x64cHKu3wOj1lM6mP&+5M4nsN;Tu&lF?VR9x66r{AU5o^^bAsz)q$9!g zJrdN;39j#vjs)in64cHK&L5;Bp%qhKJ+&2_{(7s*z7E?=xRpE&UVCK?{i^Ki$#o6) zMJst4yoSsg5-R(;cwNJN(Mp~MuZ6RQgv!3YU)OM7w2~+P4xvtX4GER~dx~blt@7_C zSVO-m`}ZwTqe{4yjt~Bhg*Ei6@+MostA8I=CEQBK{ky4pR9-{BD*N|e&4gR2KL&s2 z#Txom*}un&8dburbbRo4cdVgbmHqq3x`z9rl{^jp4w5w_RGw>Vql3S%bziiSC;#rY z9+lUSP&xk|hlE>6!)K+a!RKlDq-_3dEeSqPOFC-sxnL6XloR*kIb_n2;PcHSsGSph zzL|6+_?$KgYUc!>(@O=!@k>GnJB&eMee6NIbB=~*| z32NuW(%$!K#rrs612+SkY)`DzL6a@NYF~!{$_b3SVKa3gWp&uq0If=dA-%_Z^-8w z^h-Jt^ras3JpeQo2IoP7eo03Sjz0Pwjo>&Y9W}W70ryVeo(efZt9jRg1MP`mjq9@LHmtr%5Ka6b|54npmyK`ZLz1ouVZE+^EE z8nmKbPN>HB_u%d@)Q%doqFzpLymB1VQ`8_qD@Gd$?w-TFck-QdNN|T9(ouuHXqE2> z#2OOZfhcN_pjE!J5o<_rC#9%Cf>!wsO{^io9igHI30g_pceaWIYe-1X_6r-ZLlX&Q z?t6FTJ7$rfU(zuu`ce;ScF^838ViH-AVI&RqXtJG{fw++ z5_VVG-~HF_J;UCg=h$6R{du@5;Z{1XX9%^|Xw;Bj-?vm&THCXG+T!WS(FnJaC)cZM z`2MTX@ZD6Y*Gza+jJ8f3XZPreKz>iMvLd5iGvQH5!=sIatugfX*_B^=nz6r|uIn`u zZY2$mwobI{312w#Q*G#w-qpcHK$I!mV(y)8x{_e)E*G#yTG(6f!D8mW! z|1t9|4c|4Hdd-AKB@K_ZPWXP&^6R@lQ?Hrus2FXXusrkzN0G;JD=RYUH4`3{G(6fm zVg8GK!R1%ypRJV?edBt~gj$PGZa%otv6}yU4ubJ?uq~Xy6eDezRSNnNw1l3D@I!2t)$`QNYJlGQvHp9WkVbJE8`E8AP;Q z-S>b0!#&jqTJ?XsM6|9S^~(j#ghwK6ThB-c_a!Uqo0i6yN2qdLgM|AfT_==n>9_w| zGSX1y_212)I{m6_>lqEdL?d6avO9U#D}GUDa2~EszohGV;8%3|zwKj0{}+6k2}ISW w>x8aU_1hJS#Ab%#w`8Ot?fUO#P+f$rlN9|-x~9S3lIi;$o$&E0?U|_l2M{?a + hector_quadrotor_description + 0.3.5 + hector_quadrotor_description provides an URDF model of a quadrotor UAV. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_description + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + Stefan Kohlbrecher + + + catkin + + + + + hector_sensors_description + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.gazebo.xacro new file mode 100644 index 0000000..0db8dab --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.urdf.xacro new file mode 100644 index 0000000..d5b66ad --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box_base.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box_base.urdf.xacro new file mode 100644 index 0000000..737847b --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/box_base.urdf.xacro @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/my_robot.urdf b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/my_robot.urdf new file mode 100644 index 0000000..f0bf942 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/my_robot.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.gazebo.xacro new file mode 100644 index 0000000..005fde1 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.urdf.xacro new file mode 100644 index 0000000..5962ccd --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.gazebo.xacro new file mode 100644 index 0000000..4800ee1 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.urdf.xacro new file mode 100644 index 0000000..e7d3f21 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1_base.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1_base.urdf.xacro new file mode 100644 index 0000000..05b9c1e --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor1_base.urdf.xacro @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.1.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.1.xacro new file mode 100644 index 0000000..e70aa2b --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.1.xacro @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.xacro new file mode 100644 index 0000000..7a80f07 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_base.urdf.xacro @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.gazebo.xacro new file mode 100644 index 0000000..4908c60 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.gazebo.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.urdf.xacro new file mode 100644 index 0000000..23511c2 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.gazebo.xacro new file mode 100644 index 0000000..9a7b9a0 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.gazebo.xacro @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.urdf.xacro new file mode 100644 index 0000000..5e4f864 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_nocam.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro new file mode 100644 index 0000000..9402bbc --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro new file mode 100644 index 0000000..7707eee --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.gazebo.xacro new file mode 100644 index 0000000..e03cc8c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.urdf.xacro new file mode 100644 index 0000000..ee3fbc3 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro new file mode 100644 index 0000000..50bc643 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro new file mode 100644 index 0000000..5a30ce2 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.gazebo.xacro new file mode 100644 index 0000000..a81d98e --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.urdf.xacro new file mode 100644 index 0000000..cc43151 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.gazebo.xacro new file mode 100644 index 0000000..0460a6a --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.urdf.xacro new file mode 100644 index 0000000..8c821eb --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam1.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.gazebo.xacro new file mode 100644 index 0000000..20e55f6 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.gazebo.xacro @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.urdf.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.urdf.xacro new file mode 100644 index 0000000..30dee82 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.gv b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.gv new file mode 100644 index 0000000..c77429b --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.gv @@ -0,0 +1,14 @@ +digraph G { +node [shape=box]; +"link1" [label="link1"]; +"link2" [label="link2"]; +"link3" [label="link3"]; +"link4" [label="link4"]; +node [shape=ellipse, color=blue, fontcolor=blue]; +"link1" -> "joint1" [label="xyz: 5 3 0 \nrpy: 0 -0 0"] +"joint1" -> "link2" +"link1" -> "joint2" [label="xyz: -2 5 0 \nrpy: 0 -0 1.57"] +"joint2" -> "link3" +"link3" -> "joint3" [label="xyz: 5 0 0 \nrpy: 0 0 -1.57"] +"joint3" -> "link4" +} diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.pdf b/Flight_control/src/hector_quadrotor/hector_quadrotor_description/urdf/test_robot.pdf new file mode 100644 index 0000000000000000000000000000000000000000..820d5e1cd8ea63e32eb4c1a5d08eed137fb137d5 GIT binary patch literal 13979 zcmcJ0WmKHY(k>7P8XzGAf({nk-QC^Y8Jxl0CAd2TcXtaA+}$N;aCg@W$=>Je^PO+4 zd;i?`oq1>a>FTcP>aLnKYdu9IBOpXcOT`FBRC`!<4@VE60a)sp!f|i_s3m|PBRgXN z!wVz_2LJ%5g-p!tfHp6m=DK!30ieF60T7Oh3(nTg2B>QR=af>aBpHFpjPPS$dB+%c zC^C!7RU?B0IU|7KtqqI~4AXo~Av5j*n92Pm88z8G=3=yb*QgDi(*zP$r=je{(Q z2B$%i4R_+WxHGZJp8LKt<#p1-gcb%ce~+NQCY!1aDt^)&+sMa>Q~A zoxU@*cT|y45&;qpVUA1h;AA)$(b-ozc2629v;BAd42PmuFH-<_ygq!!uzI}dVq>nu z!@`n-eG_QdhrmJ{{qWT)bB9E`NZd-mN4@SbZPPM|VGDPy|H97&O8viZgn$ zrZ|PhhrEV@0;_q+aa`COZ7>uAIro|%g^joJ{z0|!%=D(=QkrINoNIdo>6!28Bv9CY z>{4*gA!C2+vD1EwWeKH0|f!T~`Bf5qs_x7X-?4e8g_!8 zB@KY_@BHy1^z!;k8PmU%0jL$6t$+Y(8C@gbZ^bgYHo%v8X#XBZEeSL*(dDyr0;s=0 zGypm#RsaJN6F~E~H2;gv7jav_YwG%wr-W@R?XCV{$DjFsqkk_%{za>eF38sERhPc= z->@ja&c^7#7C_5P$MSnge*iiFD-%7_KLC0FBLm}qB-}$6pp1t4B-PQ;$m$49 z12obHm?JI(8eN^pc1~DAr<2tq90te)Fz?k5DXY9bkO7f{tlO?DDYmWG_qNBa>WDjW=%!7?}+E z)HLc5)T+Y99q}N4CZHvN`aKp?n|>gC*JEZbH_G!I9cn|M&o&y%$W2X-E0u=n7+;IMeeU~6x^ zZoII)J*P5`_u+0QZ;8#<_0V>nEyH;VI5&RzuOI8KLxBSpIq|%oJRKhpmO+9bT5b>+b!Jl&oi4Nfy4*&E=8t{ofV!{{Nw6l%O1 zJD;cW!P46-^L-!|p;t$tKcGUNzy|w*0lWid2qC|Mdp`pp9`fO#3oPC|mY+Qd6>jMv zL5n<0cXwVrDiecc%xdqwwR*|~QMGa3daGGMenfCa9%cds>k`wO^*xZ#(l!MuJ>GgwFXdix}8 z@=Uo1b2eM@ByDF3^DO|hp^2sQ{L=;uc`p;<(tFN?|GiG{bM&D$;u|#fKz4*Zh;e^c zp645E@Dp!H$S24A7J`ol8xW@_PBK^NT!A4=+zC)*Oo+A)oea8^O964YbobSE80_MAK`N8;nz%l}ghdXz<+C1?)@WC?H zJH2@{JovM~GRV8I;p*?-1-HO7W8Rr1zdbB*H9zLBhM##nA}``1mpe33vjaK$ZHSaEbM@dQ2d;m-r1uUN1+ztF zcHZA%jhBO%Oq4Nbcw|<}sye$#f-udYGue3a1@hR>FnzL|V@)<|tB)~x<}n(#LUDx; ziFY$1B1KS@A+^FnitIauc{7Shwb2E)io3v=*Zl`HvTz_r?i**l>?r0Le*87Q(~#x8 zXaom$mHEZeKAj^HRQr87UU`m(8SgL0PKue=wAs||;cp*L$uw?Gu^;kJ?i8-f zS%u?XO~`|ALXW0#^;IJ5R1u-T$rgFmBb|1_+bOuXIP-fB3`07&<`BcI?!+zn%Lu#H zN@nI?W5oGuRH*p`#WkN{JS7J3lkrV%-PtZ0qhy_rhQA*_c0-BD)WQWdL1N${2~PTv zSlmdiN|)4a|8VH0ANY}!MXq?)bszqj=MeA4Z?u-rsj=@=5Yj>xg_m?r;_Lg23HiO- zc_z*#YW!*&0 zv(A_OurJ<0XM652GCGZp_%lZwQS`q=B_H6G+9H(GHABts2<}*D0Y4tn?eI%&GaIVBSfIIf}C8o>F9TLM1FtXoq1 zH>0J;2nSnmN8Gn6Mb8f%-+Xl^*SiCT3OO2ArOw4m((+M!TW(vEn>RX!T~aG&@YhWH z%5lGvJJrl`O-UsIrjm%F#xSxX(AD*z|euEAbhOag9y}x^5zG#<`t~ zLeeb|w`|ehjc_wrQq}5RNGu*KPGh@&@^ZEn73Gyx_(?mLKRdb5nJF?b*$uBitOZNA ztn(42#e+hDxO4xgTJ(6a`7BKoWj8zIz{kDs{v$z=EKs#7hv^vxHHdbkz#+MGYHL~cME90I-F zBA*5w-|_+Pzq<{Ur^iPew62CwhMir?nT+g-s^5i1N;DZ>Ll z=tgv#T=!gbnJ`^Gxa&lu8q*(WZDS!2shO-u?rOoqv_YfXRIqvzzP`$eA@OAu! z4X-5A!Q}z`FkpB;dt82RcHyaI+x55+rqQp2O;SDaEx*cAX_-N5qd!@ZAzzeD%TG*; z-W}M$(!)@X)FPC{5}R1)rz2T7PUc0AK4y_&)S zsw0_om6#6SRi=G9Zlt;^n(A%Fl`0&gv;>krqCCaRd3FwVsvO4ZS9C*N*Mpas=mCY2+vPd`5V{RR6zZW^XIth939Qb1#$$&wrWq0 zL-DAVyenQq%6RX9diNa}l?{QStt%AguPkgSOwym>?D>2xO*c-z!m`+nAJ@LEGR`Lb zw1emD@hz`c{j9GwB}?0hdGFMYg^TK&zP5zlC?CIfVz2sT#@r>N%?_G3|9iThTi9~; z?`f}^e7e;SEn?fY2}?p;L)_laF&~0rU3x*yAY1n-A0cjoy>5Lqe_?LHjlCi=CYEUF znk}yP9eZ&pqb@~-{^SpI=bVp^EK?5ix|kwuRI=wS2g2Sp{cnLujEGv4*sVVf(L8YK zbcE2iG?9@&MIZ4=&-$cx*J>HuA6cg#kq}GoL>zpIGTqV$pw9JVAr25Qr3vJR8w+Lg zMYTFLe=b%D`Ryh2x_(pmC^+8$KKg+j3^Tom`oJ9=B6{<4(?q>czHP{P3ajXG*z~yk zGOD58M^5SSJS7{uC90&-rZ+>(lECeG2IZw^YxjgvvdO|6~cYaL2K_X&8xCNS-&c!0;RvkS5g zswxK1JC^UQ-T;+K1P+vdrfF8E1;b7?ciCPV_<^m4O#R0|RYj?M)w_%l=h`5`6`4It zrQoRCuhzkuMsy|o=)-#a)*c&Jv!*6(8roS&M*|{!vo~`XOD4sdI9asx{LR+elQ$eI z!n^o+Ls$EY;o{rX$wBgCSR5@??{R34$GkYax$cH43sdQbd6C~Dh01YMEf#7rrUxi6 zqHjuG7$urSVDG!{+0d2chJ4e{A1GHwZ?K3B0+*XrsvmcrTy zwZ3lVNv1)!jeiA zR#E$1k6VkPAXl}uk0TE?k7yyO@WRu2@~a~De##?+wF!go&Ynb!MPZ^!-Qn?^(ZZzO zEivPgmD2VJH}~9~_wR&QIDqf<-IB4jyX7{<&RN!>G02$?qeR~%g^dwwARry1*cg%L ze)l5>?^Bk%y@68tBuuO2dJtR#u0P2i(uQjnFhQEg$WC~7A@V9m7-sJQaL|Kf-eg(_aJHVV( ztYlgi9*CjVZ?7!g;KsC^jOv?&6uDDp@k#rFZYCD!)nhY0iI-8F`726o@q_M!JJ8p` z`+diRB%Q@f?_lB6wkX|kxcEl6kgMgO>|}{2VHShDJf`XOv*`!=ab&I^4ut21A-qEF zlLVdwqsjF2C}16CYMKL$g3*a&lStYrT-60Q66fS~`JB2e8z9E=By5Y8o}lH8OQWh1 znN`;D&0Tm9j_`8prJhIE-j|&NmV5cx>xAips50gIinl?jESz7Vc2u&C!pia9F|Ww{ z@U~cpHU5m_TE_c1!VH?3N0NWkyi1A+J#TRuz^z2$g?1v45%+8w-%pXp&@ZQLE2Oy` zz9G@exzVRzTSlSnQ$hI$qV#|Ul!;OB3w0PY& zZyr`jMq62JW0uIawT!LLA^O$kwuvQ$?c zVuJmXGs5@cHcO252>O~vycq8`X*K6Z6UBgY2@}Fr&tQsPkVXIiThv~=8!;VjF25j9 z$WJZ#EZ>n!pAR8XCC#MF!MxpwIHDu>(k%cA&P(DKgsJIlpmc(kyJn7GL4P&2t?pQSXLV>TO`7DaPXjO@3F4vT>+mMch$ z)CBXnA$SVNGwY-GN9bI=6>0}E?>a+eA24duWKlu>=iP6gGoWE-8BOLrzUd5HAAsF2 zJ1EVzhEVcfucP1{YM6n-ITN}|?*Q!3NE`BK?lUcK48tFx*q%TWc`>Wt<8+&3{@}`R zJsj_?kR8afwQs0$chBWK;KDJ(M->{)?jnc|hCjZGiyou``6yUv$v0cH+)E*&yit3^ zbG)PrbP|g14BzZE+Fko)a?{@DFYR{DqW}JvKGlq*4*4)jOc*gDM%XmBsc{$skFGgl zrT!5Kq;JS#8hp+BmE}ba{z*_t&$wG0yDiJyEIw+6Mkm9Syf(o?DJi}M#Y5Y;xv8N0 z0po*{0i*3)8uKp_qTt=qEVHR*OX`_Dkj>&d593a6=8;~inWo6BMs}hlKE-l(K-TUk!?MhY5=r z_<~-|m0v^B&Wbwj;e%&!A!}1RI+(()>^Lt}8LM`V5Bu&=-Q5{6aGgI9ML~U!|Fn$W zXIV0)y9!-wJfLRn-L|vJ7`nt3QMgB$voReOUGQy((f91w(%uQgtvpB-1Qj1T6^*sQ zAB~S``!KRo7mH>_+hQN?iu$fd&&(E?QPjk#eu$H&N{3)}`Fb6m4>2WfX6x6DV#EMR zX)BH`t^{1wPsqnEQ5}8K;Oh=iKp(DLxMeFZMpx{?3ce(WDo~;!YShLPqH;^?z_S|T z>j;dv%-}SV?uTd?&wOI#ywD9fQq}<;*pCnDVuVGxmz0<@$sewt1Xc>oQ-$L0%HGBD z232k;yQJHtmbY>K47&E?ajy}-tAyO}D7BOe5N6RiJ*6pK9wQ}yP*lt53?*5P2-bWf zyLWwa7Bj>?FLE^lokO>mGU?m}lauh^=NOzNpvI80^fpTMf&?LdR7+gZoO2g)ZK2qF ztEui+Y>DJFj4R?NjlHlT;;K=TWaxOJv8+sMFQ@nJRPQ8d3vDe!Ts?yg*zIC;JPJG4 z$Ann_*iO`*s{|=@cH&#!eNHrBT+FtxRAR<6R{oYQNqWwMO zK<&9YU1t73HYaG#gNZw@@6!%BG=J#=vsY~D17WMZ8` z3DcOu$9v^*yf+DcVV2%-=*oNgOy-C>jqusDLzF8AfQSl_vJ=LT9pOVfFv6uRF z`}{@l#C36n7%?iXh<~;^ze%3)bNQEXSD5lGeo$19fx#w3(&$UA*V{@P&Wrry zWLJBma~B{1E770@h=$dIVp3-=*{_>kf`*=9&^kCY(M`x*@TbQcj2Z#M_g@N*a`dEd{^MnSry?mUYsVIYtWXDhU%|swCU75izArc(+Y)5VL@> zB2x~5X{076M>lzQIN7lve_-x(%jEO|5Zn0nRNQd2cjne&=@gQ?NMQb!@>qkpG+JYF z_zcs2Y;YFBw$#T!W2GiRy-C3_{e0#olZ(9JNy*4CD0Lo)G5W-~8_E>(B%ZZB+0%?u z{)DFK_r?ZmH*(GelWSpj zqgs0^xDpPI6o}+>Krb)d*O|$vFufKfY)gp$rA9 zF-Js2#Cq8yM^e)J=X*mbgN1q)I!wdZO+|b4quKA3+Y0b;q0@U#O(PE;&iyM`yYk;b zabIu)V0-HQ_8IB-x5>^hszfnT@KK0}lERqV5NbL{R(V+??n6*(=+7F;C5ok6G!tyX z56g1D1w=+=hS987eL+uF=g@m}rdV?c)Mo2ttZ!0jc!*D2{^Z&DL@}1vB=R861X4a- zGe(IjJgdJXqilk_ycz@-p+}pHE{ku~2dp$pOoB~j(nTW})V~yKTiQJED7tS#txBSkL+U&Qga(L)yXi&J$Yr^(06N0PxemjtAF zFT{D^(~R-3hGr}Za8x?VD+hA?LdYM0fK+^gle< zQn-_QY5?x)+ZEruP`@fNkPYyu$`U8(&J{ldp$4X#sCb+xFxcB^`F2*csRA{N8~3_O zE8#PR#PiIh+{IR7&8*DJMkl8hznKb5r@c9?oaaLZ^M5dyLtMx^b>RBkweZ#hj8?>f zm>@viyqPx1nzOL)UZpz}i$kSHcIzrASDPsJ_U0Hr#;t^wb@(5Su4x|!fuY)Qw7 zHTM1jimKtK&i?I5s;{bcN=95s70xF42&-;@Sxo{=_?bkr{x>te^=7VX>BNc!D zH$}z_<4lEpM}3Beo>NU2RVj_~vNv}I2n>v{)$j6sVe=ZAEjbxfwwm=zTyON=q&GHD zp0VxPZDe16sx?XwjvBX?#RsF!X>fP5qN7nsQcXXa%2qy|zWa)v@d@c>fBdCgPOcOovonUQTU$ zow!PoUb#%aEr_WooR5sr~9~|+)`+I+? zEDEL0v)U9_MII!>YT9BC{8LzZ)zwtW=BZkT8&!H?KRb0lqi6Ay`)0R8v`mjtG^oLd z4pTBnwPk#CaRjCw*99C@{}!$*iBv&-eQklVfos)m!G#7 z%avFdwz9){>g_3v=56#n2+1a?it5Tkbk_K9VO-?xMM4ns#L=EVg2~i_sXeYz=Su;h z{-Q*l6Vz7U_DV-%r@rs~0vPn4gil3bx<1UI3J3}?pVeN>n!47QK*Le)4e6`t#B@Bh z&&<`ORE4-EPm$j8sPVmf7AnZkek(#wOjdW=NMUH3L4FmYb>q>d2G1Ro;up%+YJ=;8hVg>#y8 zgj7t4&)wW*7y-XZ(lWIV=~s7H#c@VyeIa9Fg!`i+l4hs7iQV?vQW$ng%>k~7dLVg4 z-dpm4y7(r{a(JAKfqS=VqS3v;&SG1(v;OCeoSYlm2RW?k4!1y4Bi8+x1eH*`MO}{D z>s2H2tr36ri)$t`gE>z7_&USdCXN`f<1QT%lj#{|iy5f6TTHQUhiT&_vk}g_ckufjXe7u2}!y$ zn#uKX%?nCWTtdW7)by{3K;q%zvg=k7CEH#l(SwWaZ!1lSZu?w#u>!?8PB=(2dihnU z>lnnN$G(YOAMlY_OG2Tb7ca>oWe_LD3mbbtRL39^I zJ>VfRRST1T%!a>8dz{fDmGZ4jxV*4?jOV?2UvTCYq*1Z1$xcu{3%)Oob#D3~ze0`> zzB7dWRH|}V%H3D>0qLrUpIWp=!QE4IKBFU$k&r_?wIg5a3$^5=$nGfUc5dvzeG+&3 zaLGy5$!=qrw9nwCZDMkcf?Bg-b%+-Z-K^uo(PI*Q9FRsv-oa1lRwm^40xyiJ=Y02* zP}6YUX|Kj2FF5F%!e#_1Oxn$cW-L@+3(;td%`ZiM^L zo$#nqo2S!64qp5%hM;DfbZi4%)YU9A<4!`9-K6Xl*giUo4Zl zAD_<-z=9?7I!z>ti=j{N*<`9Vq(d>=jeWIZqlCV3YjP?QgF`qE&yzBnLv_mpPC~e@ zNu;QclqjoZTaCTp6BE>IVELLZa2%F&e_Xl*xg{lY&(7jxv#h}MOOcfk(NSr@EY+#v zoe|sR@O6Pu%5=bhHlkC7=D9mF_%27dgIILJ52vQDHEoI1;Gu|Lf@}GX!CMTYM8Aju zTT2dZ#DMGg4?xU)kk0Xt&1%TZsQCoxQkn4mm}9|XhhO=wV^|o;9^Scf!OjPdC*Ent zAh!boE_yYpy{dW4Ct=28;$4aQF$ek$nbmN-;q=*hzc*OI<4e7|0tzxzaxE5q#G zO+Ga>+b8%g)i*2Ut9}(3%xvM5Koi5tLBlFvo+*}l@F4khd^|M`mhV3`Rg4TT| zRUP7^KsUim6jVEPNkrLhW8d)R7`V!YU+{TD49GITV)7M=_n!OZaZZVva0n(tks}kv zYsYm0pafy^xvnb`U}NY5uNv>zQKeM2yZl$ti~)Fx$>WLCI)Lfm?nQ07LS=A4PA>dd zsN20kY9#LBl0Z2FhPwbGvIiTVuz9Xk492^AAbF01(O8C4|)jkuKhGgI9d}*oEPD~{ z-#r3B@+!YQRQB+bLq(?l7lEo-6^J8tM=EJ4J^QBt1yka!?h+ED)DX~JNb|Kddr_?} zWz{VHrQ@d_`UpYwJxm1u9Nd?`$lqwb~?wDw787yq++9Qz%GwVdqk zJ8U~Kc`3Z(NSwEeO3OS^q@<2emb`*KH`T)txoLQ=86>(u7*t3h6)Z4=*-F2o7#^ar^EX{a7Bt3^09$L zl2|%@wu)6BaeM`SLBacSSE_!?+(@VHC_mowm2Kdn$j*-h-U~Axz3Hcn<)kk{w1Q2q zxGM-6Y?K5JeDS-+t&YtR>jIDPLM2bh0r46&wAgFGN-Lqe>^PC}x$K#BAC9JZuPf5a zr^njG4aMxSn3+M!IfkN=bS-f=dY2;Rx!$0OViJDhp}W&y7#)o53R_n}za-6cwoN;4 zwBcKs*ojblH2i#B+p2O}l50!GRUah$XrJ6@#NK{&Lo+$jDS^PA?i4l@))dc9qw=`p zBkUjc*7Y7W#ZAXu>N7>*Y1~99)}>;pR)}2W;z`Lv8FAF$`PF!@-;`Fl=Wfl7@t1Wd zzi5gdjTL@TX&O0>k+53Ff z#havTkMf1Q7Xt{xLN(*S!Aevo2WNNX+J;M4u++wt`>F!j~2Dz#IEc97V=Rfs0gp_5- zL5BB_cDj1Rr*!=B88s~Yex8{@>VeRdIU3@t@PwV(2UyoMYj~?fM)jayIAI@D)YSWD z^>$ZNOk)lbtr2KK;ee(X!_n>o)Yofu@`GFQ%>-v-J*$&usMk{!az}b=V?SWvllQWE zJ|oBML*-!X2UO%cNQ>RXuEh$c0Lq;xyGk~L^P*(HR)(%ZmSibaLIcm?2au@i`ZqzW zsY=sd*mD(@(g#?8ny}ItC}EJIG3x9VYd%h+Y#pY0P6=X1^f~;&^|5bxhJDGtz^+}d zpvJ6jmvXsTKq=uGRWVO!)!UGo%ofy$D7MK2ENy+wgQmeYvJ#^ysN0W4E4y98X?jqL zfKA}jKCkRr(1Rt;ttFdn8ISTlP`JH-aS6lmF+yu)_S&M=6<=VjhFt7ye!m56k7bMB zJ7`HM#wx)seuez`*Qnyc{PBvl?UD04gqkm>7oU~&6#BbX=h#X%yEk(Wu}?ArjHZbu zKfHqQKJIsxwdiHM>G39lso(E2e;+gO~jMt-*af4^(3VXK!P0*V&toC&hvcO1yyLoabMx}uI0L3HTwbnL zD|Mto_Id}*W<58-Z$G0vaPxxs^O`Tdw=%+XB*yY|41|U_dg|2{{0U=AqKQQfL3)c>J2tgCJiTWWnnvIO<73$jWbQQOI&}AeD>E z2@kKz0LVi0!AL02mCw?#Vw!DO+AE*Ad%6pqO*$(2)(9laz6xIPpTKVICS&y>j_@P1 z6FsiRFOWPh8E2EweAnED>iKdJQ$VKbwzkonqX2nt&yUfe3Tlz26hCC_ z!9~ZTsQPG6$$6o2G~L6@?n7a7R(Hz6O|k!J@(D}Z46^XdsaM@5CV*89y5FBZu!zD; z_0pXZaN}t!W-B(^6e5}BVq;7xj?gE(MLkCH#s5pw%4!tZq;uRdw5eIzCKY=wo%W%}>k`n?5zZ5RCu z*Y__fH0>+X`ES6)A9 z^MZ`bfdHD{qj+ugU-{QBBEOrjcD%M<2mc26b*)5zCPv2p7Lm6DS||Zn{tSO@i2U7n zwfcqk3C{_2ZAD*1G6fv;2jUl(s;V{6B6tZM_H zr~5sZgzjH$TDm{Le|i@1+xLH>LJ;&)&|(5I0#GZPfOtW+CVvCJCsH;surs!OVW877 zz6R3&e$&&_|JBb(^Fn<8U*3Pv*U@y$ufhBO?|z;C_5EtaA6@^nhX(L!>)$=ER{Y(| z!0^|}cwxv}DuPU2t9$^j`Q)Fw_unV>mA!BK+f(8u2Csg4CHTwzi=%JnWo)8vOJ!+eL`wQw zkBp6_fxZ4qJ<|XCuz!~y(EhW~=}*k@>%K&SrO}_cUsv;21;W1vIoSXW;b;J~3~)66 zx&Ta!jP#5EL%<)5mY)8lGU0bG0Q46|^Ad3{$NlyCH%3E4`(GF>&8u1eAxBHcO82rm z|AEoa(!T7`|HSASU(VBi$h{nSx)(S9PmJMz+DA*r#Q5KGOw9k&K3aN)m(1}$=A&n1 zd^sF{>#(!YeF=7(-}MjjCN98NU%i$%$XQyx?Ek+jG(IqvrT^qaK-le5y OqNRf)A`+Aqg8M(eeYg++ literal 0 HcmV?d00001 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/.gitignore b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/.gitignore new file mode 100644 index 0000000..b8fb83d --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/.gitignore @@ -0,0 +1 @@ +urdf/quadrotor_plugins.gazebo.xacro diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CHANGELOG.rst new file mode 100644 index 0000000..3bc0795 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CHANGELOG.rst @@ -0,0 +1,55 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_gazebo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ +* added missing run_depend hector_quadrotor_pose_estimation to package.xml +* set pose_estimation/publish_world_nav_transform parameter to true explicitly +* updated package for the latest version of hector_pose_estimation + * Geographic reference latitude and longitude set to 49.860246N 8.687077E (Lichtwiese). + * Reenabled auto_elevation, auto_reference and auto_heading parameters for hector_pose_estimation. + hector_pose_estimation will publish the world->nav transform depending on its reference pose. +* added parameter file for hector_quadrotor_pose_estimation for a simulated quadrotor + The parameter file disables the auto_elevation, auto_reference, auto_heading modes of hector_pose_estimation and sets the corresponding values + manually with what is simulated in the gazebo sensor plugins. This simplifies the comparison of estimated poses with ground truth information. +* explicitly set the pose_estimation/nav_frame parameter in spawn_quadrotor.launch +* disabled detection of available plugins in cmake + The aerodynamics and propulsion plugins are built unconditinally now in hector_quadrotor_gazebo_plugins and the detection is obsolete. + Additionally we used platform-specific library prefixes and suffixes in find_libary() which caused errors on different platforms. +* Contributors: Johannes Meyer + +0.3.3 (2014-09-01) +------------------ +* cleaned up launch files and fixed argument forwarding to spawn_quadrotor.launch +* removed all RTT related plugins +* added separate update timer for MotorStatus output in propulsion plugin +* added launch file argument to enable/disable pose estimation +* moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo +* Contributors: Johannes Meyer + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ +* also check if a target exists when searching available plugins +* enables aerodynamics plugin in default configuration +* limit controlPeriod to 100Hz +* a few fixes for RTT integration in hector_quadrotor. Added urdf macro for rtt_gazebo_plugin macro. +* deprecated quadrotor_simple_controller.gazebo.xacro +* fixed node type for static_transform_publisher in spawn_quadrotor.launch +* changed frame_id for gazebo fixed frame to /world and added a static_transform_publisher for world->nav +* increased drift for the barometric pressure sensor +* added some command input ports to quadrotor_controller.gazebo.xacro +* Contributors: Johannes Meyer + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack +* increased drift for the barometric pressure sensor +* added some command input ports to quadrotor_controller.gazebo.xacro +* added launch file for two quadrotors diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CMakeLists.txt new file mode 100644 index 0000000..92255ca --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_gazebo) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package() + + +########### +## Build ## +########### + +add_subdirectory(urdf) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS @{name} @{name}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/a.out b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/a.out new file mode 100644 index 0000000000000000000000000000000000000000..0cdc0e33291bef5f12e99f026acdea82a03e9559 GIT binary patch literal 14167 zcmeHOeQ+DcbzcCYNPU4IEjzSqi7+E6lpTQ|qGdW3?E@)DpaV&)NIG;9`gjCD5)}xr zz(E4lIHgR>6-Je#WLii4n5m|nHttNP)l8Eu5yhxhjV_I>Qv?cVJ@{DH@}%WAPON>=tLLvDVh!Xe|zm{P}C zfwi&~OkuU`UbY;NGWb_`2~s0RbU~OVS|Q|YK$pRPtJMHntqRW&z1M)jgOR9)NYP(D zDI#moFRe9LqKr8?fO=Th^m@($9pD+F^P(OLf|4Gxs8_Lpivu0t8KU!|VnnHZ*f#v{ z5%zkl3UJ6PNZ3w(%~O=c)b*ww!2GFd&H`0D=|Grx{SJZoc>>KYq z@GW4Ara3@GR~D3CErDaboduExQ(FT6OQ=yX`{mH!V)%z8j-0>8GEA~U937--8lnG}rKjV79|G)z{ zC$}?I9UhHF)VQw2bX8^Q;|JCK!QoI`55_veT09<%Gqtm4j~WQZ1l+TyBOHwcd$hrD z&?qZj^7*~pdTS^eR|mCtu%$B+(swp_4l)(-rWOE0elG4j#$J5c|>G~`dY*RbI%)YT;WHcH8pXzWjsg4C>@n}Q~hxBpvU?Vph4aYr!TrLE; zZap0DEMj^;S`}5G8uX~1?gv%y$n{!+kwCa;wTkHWs6CCKy&d`wT8Gu>+O@ws+O2vx zaC^OY|R zM-He%S||*FKdGsj9!!RGU{Fa6`=}O*u-zRU>NZy++wJRY?@*gvExBB?>p|Ah*XM5U zRGVDe47jfkgj-;aS>X?p6;>@=n~<}@N@jyAV#`I2_9hFgT>lJnnbwo8&Eunka7sTWdq1HMla~2j!@fSQlD>E93)`Jc#o$AnZr~cmQ0Lv4@1bzU6DM^;lWE zkYA`BKry~|VSOppI0tyP9Ij}Ol2gKk30_n$;j*1Z38yxrLaT(!`;j8y=$n#P!f{Qf z)Ft825xLYS;n1PEG$7%tb0}jY5)K`lOJfpl&!LPZCA?C?CnOyAA4EiqijzmBP~RgW%1f%G#Zbo&7lsOs{wA91!p1g58^X^N_WF<}BFb z19G$fJQiiIRMt))b{093pI)$myf^i-clz36UdycahYJZ%ehCG=M_<)%_Ip#W82VR* z{zRJ_>dO*Uy`Y)?GBl|Fl`OACF`gLe%>SB5r$T={|&SSr&?}gEaI-fXt zsp*$~v}e$fy7d0F=?|BuE=@1gZh1L18=6Yn7~3+Px^$!R{;3<4jy30Cp5CZ@ zYI>-$9xzAQ>vuSTcii~{PQUo_jmjsdxg6*FAYXNK7Tl%(1}?YlrO)K$nDCM`c^Q$s z%z*>yy%Ev&sgJVR<9~w9O;nGWAyN3($WYf_28OgJgupYhz&}X>|0)DDz-069 z{0c0-B?@=S+J_|Vc_HvgllC`d?d7ue!;*GQwK1%3qBPaMMiOWj0vA94Z;a!-|GlZB zXQ3NlXgKwz{w=*t_VCnbS# zAy6g@v`PZh=8b?&@~Lc@{1+VJJ1U=Vl;*%Q!qB&|UZnIkFrsBRAI<=4+~vpn(*f_c zd-?sVZ+vULH#H;49YeL$9ID)cY?|{&F5q4Nh5i{_1Yj3&q@MmLl~|a3`Xfu?Q%9a& zzD_>g=us=KBgUtxWZZ@;vZo<9!rjfod@ArIuwnx)it>q>mGZQ?J!Co1^FFX zY}gsmMuTm2`0yRBa|VJ#S|Y5s)orXFf`?G&NYJlGW9siEv_LG1xj-=PkA=o`u(+im z5ep18z}uFuifAPmm70N#sdFCG!U?d;ee!zb+o=z!W3k{+C|Mw7R7*A_`a@XS zSdSJsw#8^!-eTU8tUsQ!&vrD3=0Z&yc#mKvKMKOR@)gB$kL?j3u=pTR|9&=m3TPip z+7X~0m^AqC`Z(NA^+5j+?<=5x0dxw=aVfzko_g3i&H%-<)*#Gsct5iw9hQ4mmsgy$ zlsgc|Gp027xL?R1DK5|0Mkqu1WAA0NjcD8M*k!MIymHl{iV61Ex<~Hcwy_RSE(4Db z>@`^XxGlHc@tn0|&GLQVSd^oj<#3me*Qne!1-G5esF&dfeYHWG%a*~LL13-I$cA^) zW?QgvLs*x;hyQiprwwRMCYks1eSjZ=a{e4jHoA;*tmAd~_dp zjFin<*7{)29C&T{Ps-W3a>%}em!-7S14})y)B{UBu+#%fJ+RaROFi)0>wzHF$Ef_DhIPS6GM9TuNs zG2yc@rvLsp8%27`svyaI^PO8}@N~9($!hR;ponR&c=)8V;Ti*G@);~bHi0vE;l)JH ztmgLxI;;K!mxAx&%qS5@69tS4WwBsbLj{o=3+v1kf^Uq>1s}NJ3P%$mD2oO(U1P4`h&mL5 z_kMLG0J_A1QTV17O!|Xkx(d~Y@0r|&s>Nd3xM73xzv~A@@TZN2{Gb;F7jSWLzlM*& zc$~TX(b3UhL|-%@zczsD0xl_Z|Fv2b;PP=Hv@UQEF0>92|4GQ;y-4~!Rt30x+=Y{K z`3t;C7o;ZOWPRPL@N!)k#CLExnf3Ag8IA&?Bc9f`KA}G$^sxR|PpZFw*b9}?Ii!CPx!MYeOjOAg+8t4 zsE>P=S^s&!uuo_|q5bT%SiotY0ui1>w-U=zkdyVN#5y@8^!XW4PCEbnmiECvHtAE` z)x6lyUZsW$8>B}j|IDOM`|kxINc#}Uo9lmC=u`jCiv4?5=wC8T80tT=_o7Lk;*=1u^brmHz&U;01%Pl1@MkH=(~0&6+R?Y{6KNl& z=M{>FGWWN+{jY;AetaZ-dY)NW4Oj7Y&lTk{;3Jpe*aZ=p5j( zFFFmCMuX(8q({JNP)3aU4?n+RnsaipsQ=NIoT`L>vmesmBMU>G^oin!W%4bHZo1Fl z){&qnG_l+s&3##)iNokV(O{eoxK~m8Y5l|dyO{qzcxu6vHQjJ#|AtiYs|{+`YE>2$ zV$2KevvMIu#?by*h%d`M#~0#du+J$n#Pb+UL;ILqXoH=a_NPL;g3-QHh_7I@pA_ON zbIu@1n$!L8o#2vXf&qBP4(Rx^jSHt?I$Pmxl zAYzi%FS*bLr}MNf72@z#MC*-IU}JaX))5JY(ftIY^`Q`l*(mP2Li}z<_j4h>4(`tZ z8RB^xqqoF-5zg0T>>VBukVlQZ+=6d)ruyT1rq+;`5XTqy=Lunl-m`(_=}+NmDKai+0Wa3B?{N8b3_rwTBEPRe9@}-bQbCxt@hjK> z0mSLOp5Aj&ubBNBD6p_9Hei}Rn>oI?KhaGwJI!2v@wnr6IE>si&$lN9Zl0fEZfEg) zO91Xwr9;MMR1 zVZPu0i{p|3k-Y;dP^|vVfY-q|wupL)#~0=+AwMK={8lfgE)CJb+h2|6rIF zrXasE*2{6hNaU+%2kLW4*rEEoAn;ie{^x)@r2CyreFN}f<26$Pzbx!raSo6PPJdcL z{(};@7X||To;Ho|Nr4v_2j;H;$8j;QtA8itr%ZOfSHjNK68KGyuVEM;hY7z)WBP#O zMfOd9Opogch}3{MWA*Wl{i?6Cy9Xk#^8wbXKC1dL#5&F>vRZ;vH4s&Y!_h%4tOg+5 zBCcwQB!hE@v2aii23*@8erOw5E+VAjnTTe;&!!_axph`f;Aw`*6o2O_QA?LH4m6jTwy zzGZ-ivom$)lY8BJIy)F1&=|4Kxd82eqVPy(5y(t&+aVKdgh~733Ep{(jNC@hJU@Wq z!S0L0xr;h6LhvybTNFbOHilkf6niwbC=9+E`ye0DZPqCod~fpmzs2?cW})+D2m7GI z0-=bShzDVW&>zj{!e0@n(g^YCkQ~*L&_0mL&4@res*b>P!e1Taheb@l+x<-e2H6)@ zavW#hxZw_t@Ny$u7(Jiw-WJA#?DGI0BlxvIZfa5Nf1#iM_wOFe=DPLY>^BO9cJK!S l=C<(j%woT?P!#ER0EIt702|^51o-QNLW}(O1^o78{{xD}?cD$X literal 0 HcmV?d00001 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/cxf_myspawn.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/cxf_myspawn.launch new file mode 100644 index 0000000..a54e411 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/cxf_myspawn.launch @@ -0,0 +1,204 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/generate.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/generate.cpp new file mode 100644 index 0000000..74d3d83 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/generate.cpp @@ -0,0 +1,35 @@ +#include +#include +#include +#include + +using namespace std; +#define N 20 +double xmin=-60,xmax=-20,ymin=-60,ymax=-20; + +int main() +{ + srand(time(0)); + ofstream fout("myspawn.launch"); + fout<<""<"<"<"<"<"<"<"<"<"<"<"<"< + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/nine_quadrotor_empty_world.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/nine_quadrotor_empty_world.launch new file mode 100644 index 0000000..d6a4653 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/nine_quadrotor_empty_world.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/quadrotor_empty_world.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/quadrotor_empty_world.launch new file mode 100644 index 0000000..9c3256a --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/quadrotor_empty_world.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_box.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_box.launch new file mode 100644 index 0000000..95a03a3 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_box.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_nine_quadrotors.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_nine_quadrotors.launch new file mode 100644 index 0000000..b140fb5 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_nine_quadrotors.launch @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor (另一个复件).launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor (另一个复件).launch new file mode 100644 index 0000000..89b7493 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor (另一个复件).launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(backup).launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(backup).launch new file mode 100644 index 0000000..89b7493 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(backup).launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(zenglei).launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(zenglei).launch new file mode 100644 index 0000000..d727a03 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor(zenglei).launch @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch new file mode 100644 index 0000000..3a09319 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_slam.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_slam.launch new file mode 100644 index 0000000..4532502 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_slam.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_split.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_split.launch new file mode 100644 index 0000000..793b2f4 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_split.launch @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus.launch new file mode 100644 index 0000000..56a7d9a --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus_with_laser.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus_with_laser.launch new file mode 100644 index 0000000..03783bf --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_asus_with_laser.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_cam.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_cam.launch new file mode 100644 index 0000000..9c4a336 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_cam.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam.launch new file mode 100644 index 0000000..46b0c9d --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_downward_cam.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_kinect.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_kinect.launch new file mode 100644 index 0000000..82d5a11 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_kinect.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_laser.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_laser.launch new file mode 100644 index 0000000..a3a7850 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_laser.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_ten_quadrotors.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_ten_quadrotors.launch new file mode 100644 index 0000000..398d49d --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_ten_quadrotors.launch @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_three_quadrotors.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_three_quadrotors.launch new file mode 100644 index 0000000..6650c34 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_three_quadrotors.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_two_quadrotors.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_two_quadrotors.launch new file mode 100644 index 0000000..acd6ac3 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_two_quadrotors.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/water_myspawn.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/water_myspawn.launch new file mode 100644 index 0000000..2ce6964 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/water_myspawn.launch @@ -0,0 +1,204 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/package.xml new file mode 100644 index 0000000..eb570c0 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/package.xml @@ -0,0 +1,44 @@ + + hector_quadrotor_gazebo + 0.3.5 + hector_quadrotor_gazebo provides a quadrotor model for the gazebo simulator. + It can be commanded using geometry_msgs/Twist messages. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_gazebo + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + Stefan Kohlbrecher + + + catkin + + + gazebo_plugins + hector_gazebo_plugins + hector_sensors_gazebo + hector_quadrotor_gazebo_plugins + hector_quadrotor_controller_gazebo + message_to_tf + robot_state_publisher + + + gazebo_plugins + hector_gazebo_plugins + hector_sensors_gazebo + hector_quadrotor_description + hector_quadrotor_model + hector_quadrotor_gazebo_plugins + hector_quadrotor_controller_gazebo + hector_quadrotor_pose_estimation + hector_quadrotor_teleop + hector_uav_msgs + message_to_tf + robot_state_publisher + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/CMakeLists.txt new file mode 100644 index 0000000..5cdebd7 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/CMakeLists.txt @@ -0,0 +1,49 @@ +# This CMakeLists.txt configures the plugins used for the quadrotor model. +set(MODEL_PLUGINS) + +# configuration +option(USE_EXTERNAL_CONTROLLER "Do not run the hector_quadrotor_controller_gazebo plugin." OFF) +option(USE_PROPULSION_PLUGIN "Use a model of the quadrotor propulsion system" ON) +option(USE_AERODYNAMICS_PLUGIN "Use a model of the quadrotor aerodynamics" ON) + +# sensor plugins +list(APPEND MODEL_PLUGINS quadrotor_sensors) + +# controller plugin +if(NOT USE_EXTERNAL_CONTROLLER) + list(APPEND MODEL_PLUGINS quadrotor_controller) +endif() + +# propulsion plugin +#if(USE_PROPULSION_PLUGIN) +# find_library(hector_gazebo_quadrotor_propulsion_LIBRARY hector_gazebo_quadrotor_propulsion) + +# if(NOT TARGET hector_gazebo_quadrotor_propulsion AND NOT hector_gazebo_quadrotor_propulsion_LIBRARY) +# message(SEND_ERROR "Cannot use the propulsion model as the required Gazebo plugin 'hector_gazebo_quadrotor_propulsion' has not been found.") +# set(USE_PROPULSION_PLUGIN OFF) +# endif() +#endif() +if(USE_PROPULSION_PLUGIN) + list(APPEND MODEL_PLUGINS quadrotor_propulsion) +endif() + +# aerodynamics plugin +#if(USE_AERODYNAMICS_PLUGIN) +# find_library(hector_gazebo_quadrotor_aerodynamics_LIBRARY hector_gazebo_quadrotor_aerodynamics) + +# if(NOT TARGET hector_gazebo_quadrotor_aerodynamics AND NOT hector_gazebo_quadrotor_aerodynamics_LIBRARY) +# message(SEND_ERROR "Cannot use the aerodynamics model as the required Gazebo plugin 'hector_gazebo_quadrotor_aerodynamics' has not been found.") +# set(USE_AERODYNAMICS_PLUGIN OFF) +# endif() +#endif() +if(USE_AERODYNAMICS_PLUGIN) + list(APPEND MODEL_PLUGINS quadrotor_aerodynamics) +endif() + +# write urdf +set(MODEL_PLUGINS_URDF) +foreach(PLUGIN ${MODEL_PLUGINS}) + set(MODEL_PLUGINS_URDF "${MODEL_PLUGINS_URDF} \n") +endforeach() +configure_file(quadrotor_plugins.gazebo.xacro.in ${PROJECT_SOURCE_DIR}/urdf/quadrotor_plugins.gazebo.xacro @ONLY) +set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES ${PROJECT_SOURCE_DIR}/urdf/quadrotor_plugins.gazebo.xacro) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_aerodynamics.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_aerodynamics.gazebo.xacro new file mode 100644 index 0000000..29c3587 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_aerodynamics.gazebo.xacro @@ -0,0 +1,18 @@ + + + + + + + + + true + 0.0 + base_link + $(arg base_link_frame) + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_controller.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_controller.gazebo.xacro new file mode 100644 index 0000000..4330c95 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_controller.gazebo.xacro @@ -0,0 +1,11 @@ + + + + + + 0.01 + hector_quadrotor_controller_gazebo/QuadrotorHardwareSim + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro.in b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro.in new file mode 100644 index 0000000..b17d7d6 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro.in @@ -0,0 +1,16 @@ + + + + + + + + + + + + + +@MODEL_PLUGINS_URDF@ + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_propulsion.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_propulsion.gazebo.xacro new file mode 100644 index 0000000..b4c08fe --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_propulsion.gazebo.xacro @@ -0,0 +1,22 @@ + + + + + + + + + true + 0.0 + base_link + $(arg base_link_frame) + 100.0 + + 0.01 + 100.0 + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_sensors.gazebo.xacro b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_sensors.gazebo.xacro new file mode 100644 index 0000000..687c358 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_sensors.gazebo.xacro @@ -0,0 +1,69 @@ + + + + + + + + + 100.0 + base_link + $(arg base_link_frame) + raw_imu + 0 0 0 + + 0 + + 0.5 0.5 0.5 + 0.35 0.35 0.3 + 0.1 0.1 0.1 + 0.05 0.05 0.015 + + + + 10.0 + base_link + $(arg base_link_frame) + pressure_height + altimeter + 0 + 10.0 + 0.1 + + + + 10.0 + base_link + $(arg base_link_frame) + magnetic + 0 0 0 + 0.0 0.0 0.0 + 1.3e-2 1.3e-2 1.3e-2 + + + + 4.0 + base_link + $(arg base_link_frame) + fix + fix_velocity + 49.860246 + 8.687077 + 5.0 5.0 5.0 + 0.01 0.01 0.01 + 0 0 0 + 0.05 0.05 0.05 + + + + 100.0 + base_link + ground_truth/state + 0.0 + $(arg world_frame) + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CHANGELOG.rst new file mode 100644 index 0000000..4976229 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CHANGELOG.rst @@ -0,0 +1,50 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_gazebo_plugins +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ +* added dynamic_reconfigure server to gazebo_ros_baro plugin + See https://github.com/tu-darmstadt-ros-pkg/hector_gazebo/commit/e1698e1c7bfa5fce6a724ab0a922a88bd49c9733 for + the equivalent commit in hector_gazebo_plugins. +* publish propulsion and aerodynamic wrench as WrenchStamped + This is primarily for debugging purposes. + The default topic for the propulsion plugin has been changed to propulsion/wrench. +* disabled detection of available plugins in cmake + The aerodynamics and propulsion plugins are built unconditinally now in hector_quadrotor_gazebo_plugins and the detection is obsolete. + Additionally we used platform-specific library prefixes and suffixes in find_libary() which caused errors on different platforms. +* Contributors: Johannes Meyer + +0.3.3 (2014-09-01) +------------------ +* fixed some compiler warnings and missing return values +* added separate update timer for MotorStatus output in propulsion plugin +* Contributors: Johannes Meyer + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ +* disabled separate queue thread for the aerodynamics plugin +* fixed configuration namespace and plugin cleanup +* aerodynamics plugin should apply forces and torques in world frame +* accept hector_uav_msgs/MotorCommand messages directly for the propulsion model/plugin +* deleted deprecated export section from package.xml +* abort with a fatal error if ROS is not yet initialized + minor code cleanup +* fixed commanded linear z velocity upper bound for auto shutdown in simple controller plugin +* improved auto shutdown to prevent shutdowns while airborne +* added motor engage/shutdown, either automatically (default) or using ROS services /engage and /shutdown + (std_srvs/Empty) +* using ROS parameters to configure state topics +* use controller_manager in gazebo_ros_control instead of running standalone pose_controller +* Contributors: Johannes Meyer + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack +* added wrench publisher to the quadrotor_simple_controller plugin +* created new package hector_quadrotor_model and moved all gazebo plugins to hector_quadrotor_gazebo_plugins diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CMakeLists.txt new file mode 100644 index 0000000..5d5ce70 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_gazebo_plugins) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp hector_gazebo_plugins hector_quadrotor_model geometry_msgs hector_uav_msgs dynamic_reconfigure) +include_directories(include ${catkin_INCLUDE_DIRS}) + +# Depend on system install of Gazebo +find_package(gazebo REQUIRED) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") +link_directories(${GAZEBO_LIBRARY_DIRS}) +include_directories(${GAZEBO_INCLUDE_DIRS}) + +## Find hector_quadrotor_model optional libraries +if(TARGET hector_quadrotor_propulsion) + set(hector_quadrotor_propulsion_LIBRARY hector_quadrotor_propulsion) +else() + find_library(hector_quadrotor_propulsion_LIBRARY hector_quadrotor_propulsion) +endif() +if(TARGET hector_quadrotor_propulsion) + set(hector_quadrotor_aerodynamics_LIBRARY hector_quadrotor_aerodynamics) +else() + find_library(hector_quadrotor_aerodynamics_LIBRARY hector_quadrotor_aerodynamics) +endif() + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS thread) +# include_directories(${Boost_INCLUDE_DIRS}) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES + CATKIN_DEPENDS roscpp hector_gazebo_plugins hector_quadrotor_model geometry_msgs hector_uav_msgs dynamic_reconfigure + DEPENDS gazebo +) + +########### +## Build ## +########### + +add_library(hector_gazebo_ros_baro src/gazebo_ros_baro.cpp) +target_link_libraries(hector_gazebo_ros_baro ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES}) +add_dependencies(hector_gazebo_ros_baro hector_uav_msgs_generate_messages_cpp) + +add_library(hector_gazebo_quadrotor_simple_controller src/gazebo_quadrotor_simple_controller.cpp) +target_link_libraries(hector_gazebo_quadrotor_simple_controller ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES}) + +if(hector_quadrotor_propulsion_LIBRARY) + add_library(hector_gazebo_quadrotor_propulsion src/gazebo_quadrotor_propulsion.cpp) + target_link_libraries(hector_gazebo_quadrotor_propulsion ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${hector_quadrotor_propulsion_LIBRARY}) + add_dependencies(hector_gazebo_quadrotor_propulsion hector_uav_msgs_generate_messages_cpp) +else() + message(WARNING "Quadrotor propulsion model is not available. Skipping target hector_gazebo_quadrotor_propulsion...") +endif() + +if(hector_quadrotor_aerodynamics_LIBRARY) + add_library(hector_gazebo_quadrotor_aerodynamics src/gazebo_quadrotor_aerodynamics.cpp) + target_link_libraries(hector_gazebo_quadrotor_aerodynamics ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${hector_quadrotor_aerodynamics_LIBRARY}) +else() + message(WARNING "Quadrotor aerodynamics model is not available. Skipping target hector_gazebo_quadrotor_aerodynamics...") +endif() + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS hector_gazebo_ros_baro hector_gazebo_quadrotor_simple_controller + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +if(TARGET hector_gazebo_quadrotor_propulsion) + install(TARGETS hector_gazebo_quadrotor_propulsion LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +endif() + +if(TARGET hector_gazebo_quadrotor_aerodynamics) + install(TARGETS hector_gazebo_quadrotor_aerodynamics LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +endif() + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_aerodynamics.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_aerodynamics.h new file mode 100644 index 0000000..bbc7a04 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_aerodynamics.h @@ -0,0 +1,90 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_AERODYNAMICS_H +#define HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_AERODYNAMICS_H + +#include +#include +#include + +#include +#include + +#include + +#include + +namespace gazebo +{ + +class GazeboQuadrotorAerodynamics : public ModelPlugin +{ +public: + GazeboQuadrotorAerodynamics(); + virtual ~GazeboQuadrotorAerodynamics(); + +protected: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void Reset(); + virtual void Update(); + +private: + /// \brief The parent World + physics::WorldPtr world; + + /// \brief The link referred to by this plugin + physics::LinkPtr link; + + hector_quadrotor_model::QuadrotorAerodynamics model_; + + ros::NodeHandle* node_handle_; + ros::CallbackQueue callback_queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + ros::Subscriber wind_subscriber_; + ros::Publisher wrench_publisher_; + + std::string body_name_; + std::string namespace_; + std::string param_namespace_; + double control_rate_; + std::string wind_topic_; + std::string wrench_topic_; + std::string frame_id_; + + common::Time last_time_; + + // Pointer to the update event connection + event::ConnectionPtr updateConnection; +}; + +} + +#endif // HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_AERODYNAMICS_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_propulsion.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_propulsion.h new file mode 100644 index 0000000..47a97ba --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_propulsion.h @@ -0,0 +1,106 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_PROPULSION_H +#define HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_PROPULSION_H + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace gazebo +{ + +class GazeboQuadrotorPropulsion : public ModelPlugin +{ +public: + GazeboQuadrotorPropulsion(); + virtual ~GazeboQuadrotorPropulsion(); + +protected: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void Reset(); + virtual void Update(); + +private: + /// \brief The parent World + physics::WorldPtr world; + + /// \brief The link referred to by this plugin + physics::LinkPtr link; + + hector_quadrotor_model::QuadrotorPropulsion model_; + + ros::NodeHandle* node_handle_; + ros::CallbackQueue callback_queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + ros::Publisher trigger_publisher_; + ros::Subscriber command_subscriber_; + ros::Subscriber pwm_subscriber_; + ros::Publisher wrench_publisher_; + ros::Publisher supply_publisher_; + ros::Publisher motor_status_publisher_; + + std::string body_name_; + std::string namespace_; + std::string param_namespace_; + std::string trigger_topic_; + std::string command_topic_; + std::string pwm_topic_; + std::string wrench_topic_; + std::string supply_topic_; + std::string status_topic_; + std::string frame_id_; + ros::Duration control_delay_; + ros::Duration control_tolerance_; + + common::Time last_time_; + common::Time last_trigger_time_; + common::Time last_motor_status_time_; + common::Time last_supply_time_; + + // Pointer to the update event connection + event::ConnectionPtr updateConnection; + + UpdateTimer controlTimer; + UpdateTimer motorStatusTimer; +}; + +} + +#endif // HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_PROPULSION_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_simple_controller.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_simple_controller.h new file mode 100644 index 0000000..c3a2596 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_quadrotor_simple_controller.h @@ -0,0 +1,143 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H +#define HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +namespace gazebo +{ + +class GazeboQuadrotorSimpleController : public ModelPlugin +{ +public: + GazeboQuadrotorSimpleController(); + virtual ~GazeboQuadrotorSimpleController(); + +protected: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void Update(); + virtual void Reset(); + +private: + /// \brief The parent World + physics::WorldPtr world; + + /// \brief The link referred to by this plugin + physics::LinkPtr link; + + ros::NodeHandle* node_handle_; + ros::CallbackQueue callback_queue_; + ros::Subscriber velocity_subscriber_; + ros::Subscriber imu_subscriber_; + ros::Subscriber state_subscriber_; + ros::Publisher wrench_publisher_; + + ros::ServiceServer engage_service_server_; + ros::ServiceServer shutdown_service_server_; + + // void CallbackQueueThread(); + // boost::mutex lock_; + // boost::thread callback_queue_thread_; + + geometry_msgs::Twist velocity_command_; + void VelocityCallback(const geometry_msgs::TwistConstPtr&); + void ImuCallback(const sensor_msgs::ImuConstPtr&); + void StateCallback(const nav_msgs::OdometryConstPtr&); + + bool EngageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + bool ShutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); + + ros::Time state_stamp; + math::Pose pose; + math::Vector3 euler, velocity, acceleration, angular_velocity; + + std::string link_name_; + std::string namespace_; + std::string velocity_topic_; + std::string imu_topic_; + std::string state_topic_; + std::string wrench_topic_; + double max_force_; + + bool running_; + bool auto_engage_; + + class PIDController { + public: + PIDController(); + virtual ~PIDController(); + virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = ""); + + double gain_p; + double gain_i; + double gain_d; + double time_constant; + double limit; + + double input; + double dinput; + double output; + double p, i, d; + + double update(double input, double x, double dx, double dt); + void reset(); + }; + + struct Controllers { + PIDController roll; + PIDController pitch; + PIDController yaw; + PIDController velocity_x; + PIDController velocity_y; + PIDController velocity_z; + } controllers_; + + math::Vector3 inertia; + double mass; + + math::Vector3 force, torque; + + UpdateTimer controlTimer; + event::ConnectionPtr updateConnection; +}; + +} + +#endif // HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h new file mode 100644 index 0000000..3c8ab18 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h @@ -0,0 +1,90 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_BARO_H +#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_BARO_H + +#include + +#include +#include +#include + +#include +#include + +#include + +namespace gazebo +{ + +class GazeboRosBaro : public ModelPlugin +{ +public: + GazeboRosBaro(); + virtual ~GazeboRosBaro(); + +protected: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void Reset(); + virtual void Update(); + +private: + /// \brief The parent World + physics::WorldPtr world; + + /// \brief The link referred to by this plugin + physics::LinkPtr link; + + ros::NodeHandle* node_handle_; + ros::Publisher height_publisher_; + ros::Publisher altimeter_publisher_; + + geometry_msgs::PointStamped height_; + hector_uav_msgs::Altimeter altimeter_; + + std::string namespace_; + std::string height_topic_; + std::string altimeter_topic_; + std::string link_name_; + std::string frame_id_; + + double elevation_; + double qnh_; + + SensorModel sensor_model_; + + UpdateTimer updateTimer; + event::ConnectionPtr updateConnection; + + boost::shared_ptr > dynamic_reconfigure_server_; +}; + +} // namespace gazebo + +#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_BARO_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/package.xml new file mode 100644 index 0000000..6d16192 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/package.xml @@ -0,0 +1,40 @@ + + hector_quadrotor_gazebo_plugins + 0.3.5 + hector_quadrotor_gazebo_plugins provides gazebo plugins for using quadrotors in gazebo. + The hector_gazebo_ros_baro sensor plugin simulates an altimeter based on barometric pressure. + hector_quadrotor_simple_controller is a simple controller allowing to command the quadrotor's velocity + using a geometry_msgs/Twist message for teleoperation just by means of applying forces and torques to the model. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_gazebo_plugins + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + + + catkin + + + roscpp + gazebo + hector_gazebo_plugins + hector_quadrotor_model + geometry_msgs + hector_uav_msgs + std_srvs + dynamic_reconfigure + + + roscpp + gazebo_ros + hector_gazebo_plugins + hector_quadrotor_model + geometry_msgs + hector_uav_msgs + std_srvs + dynamic_reconfigure + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_aerodynamics.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_aerodynamics.cpp new file mode 100644 index 0000000..f69365c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_aerodynamics.cpp @@ -0,0 +1,236 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include +#include + +#include + +#ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.19 +#define _COUNT_TIME_HH_ + +#ifndef _WIN32 +#include +#endif + +#ifndef USE_COUNT_TIME +#define USE_COUNT_TIME +#endif + +#endif + +namespace gazebo +{ + +using namespace common; +using namespace math; +using namespace hector_quadrotor_model; + +GazeboQuadrotorAerodynamics::GazeboQuadrotorAerodynamics() + : node_handle_(0) +{ +} + +GazeboQuadrotorAerodynamics::~GazeboQuadrotorAerodynamics() +{ + event::Events::DisconnectWorldUpdateBegin(updateConnection); + if (node_handle_) + { + node_handle_->shutdown(); + if (callback_queue_thread_.joinable()) + callback_queue_thread_.join(); + delete node_handle_; + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboQuadrotorAerodynamics::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + world = _model->GetWorld(); + link = _model->GetLink(); + + // default parameters + namespace_.clear(); + param_namespace_ = "quadrotor_aerodynamics"; + wind_topic_ = "/wind"; + wrench_topic_ = "aerodynamics/wrench"; + frame_id_ = link->GetName(); + + // load parameters + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + if (_sdf->HasElement("paramNamespace")) + param_namespace_ = _sdf->GetElement("paramNamespace")->Get(); + if (_sdf->HasElement("windTopicName")) + wind_topic_ = _sdf->GetElement("windTopicName")->Get(); + if (_sdf->HasElement("wrenchTopic")) + wrench_topic_ = _sdf->GetElement("wrenchTopic")->Get(); + if (_sdf->HasElement("frameId")) + frame_id_ = _sdf->GetElement("frameId")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + node_handle_ = new ros::NodeHandle(namespace_); + + // get model parameters + if (!model_.configure(ros::NodeHandle(*node_handle_, param_namespace_))) + { + gzwarn << "[quadrotor_propulsion] Could not properly configure the aerodynamics plugin. Make sure you loaded the parameter file." << std::endl; + return; + } + + // subscribe command + if (!wind_topic_.empty()) + { + ros::SubscribeOptions ops; + ops.callback_queue = &callback_queue_; + ops.initByFullCallbackType( + wind_topic_, 1, + boost::bind(&QuadrotorAerodynamics::setWind, &model_, _1)); + wind_subscriber_ = node_handle_->subscribe(ops); + } + + // advertise wrench + if (!wrench_topic_.empty()) + { + ros::AdvertiseOptions ops; + ops.callback_queue = &callback_queue_; + ops.init(wrench_topic_, 10); + wrench_publisher_ = node_handle_->advertise(ops); + } + + // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorAerodynamics::QueueThread,this ) ); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboQuadrotorAerodynamics::Update, this)); +} + +#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 +double GazeboQuadrotorAerodynamics_Update = 0.0; +int num = 0; +#endif +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboQuadrotorAerodynamics::Update() +{ +#ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19 + struct timeval tv; + double start_time, cur_time, tmp_time; + gettimeofday(&tv, NULL); + start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +#endif + std::cout<<"=====================GazeboQuadrotorAerodynamics::Update========================"<GetSimTime(); + Time dt = current_time - last_time_; + last_time_ = current_time; + if (dt <= 0.0) + return; + + // Get new commands/state + callback_queue_.callAvailable(); + + // fill input vector u for drag model + geometry_msgs::Quaternion orientation; + fromQuaternion(link->GetWorldPose().rot, orientation); + model_.setOrientation(orientation); + + geometry_msgs::Twist twist; + fromVector(link->GetWorldLinearVel(), twist.linear); + fromVector(link->GetWorldAngularVel(), twist.angular); + model_.setTwist(twist); + + // update the model + model_.update(dt.Double()); + + // get wrench from model + Vector3 force, torque; + toVector(model_.getWrench().force, force); + toVector(model_.getWrench().torque, torque); + + // publish wrench + if (wrench_publisher_) + { + geometry_msgs::WrenchStamped wrench_msg; + wrench_msg.header.stamp = ros::Time(current_time.sec, current_time.nsec); + wrench_msg.header.frame_id = frame_id_; + wrench_msg.wrench = model_.getWrench(); + wrench_publisher_.publish(wrench_msg); + } + + // set force and torque in gazebo + link->AddRelativeForce(force); + link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force)); + +#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 + // gettimeofday(&tv, NULL); + // GazeboQuadrotorAerodynamics_Update += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; + // num++; + // if (num == 100000) + // { + // std::cout << "GazeboQuadrotorAerodynamics_Update: " << GazeboQuadrotorAerodynamics_Update << "num:" << num << std::endl; + // } +#endif +} + +//////////////////////////////////////////////////////////////////////////////// +// Reset the controller +void GazeboQuadrotorAerodynamics::Reset() +{ + model_.reset(); +} + +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboQuadrotorAerodynamics::QueueThread() +{ + static const double timeout = 0.01; + + while (node_handle_->ok()) + { + callback_queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorAerodynamics) + +} // namespace gazebo diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_propulsion.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_propulsion.cpp new file mode 100644 index 0000000..8d71530 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_propulsion.cpp @@ -0,0 +1,340 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include +#include + +#include +#include + +// #ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.20 +// #define _COUNT_TIME_HH_ + +// #ifndef _WIN32 +// #include +// #endif + +// #ifndef USE_COUNT_TIME +// #define USE_COUNT_TIME +// #endif + +// #endif + +namespace gazebo +{ + +using namespace common; +using namespace math; +using namespace hector_quadrotor_model; + +GazeboQuadrotorPropulsion::GazeboQuadrotorPropulsion() + : node_handle_(0) +{ +} + +GazeboQuadrotorPropulsion::~GazeboQuadrotorPropulsion() +{ + event::Events::DisconnectWorldUpdateBegin(updateConnection); + if (node_handle_) + { + node_handle_->shutdown(); + if (callback_queue_thread_.joinable()) + callback_queue_thread_.join(); + delete node_handle_; + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboQuadrotorPropulsion::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + world = _model->GetWorld(); + link = _model->GetLink(); + + // default parameters + namespace_.clear(); + param_namespace_ = "quadrotor_propulsion"; + trigger_topic_ = "quadro/trigger"; + command_topic_ = "command/motor"; + pwm_topic_ = "motor_pwm"; + wrench_topic_ = "propulsion/wrench"; + supply_topic_ = "supply"; + status_topic_ = "motor_status"; + control_tolerance_ = ros::Duration(); + control_delay_ = ros::Duration(); + frame_id_ = link->GetName(); + + // load parameters + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + if (_sdf->HasElement("paramNamespace")) + param_namespace_ = _sdf->GetElement("paramNamespace")->Get(); + if (_sdf->HasElement("triggerTopic")) + trigger_topic_ = _sdf->GetElement("triggerTopic")->Get(); + if (_sdf->HasElement("topicName")) + command_topic_ = _sdf->GetElement("topicName")->Get(); + if (_sdf->HasElement("pwmTopicName")) + pwm_topic_ = _sdf->GetElement("pwmTopicName")->Get(); + if (_sdf->HasElement("wrenchTopic")) + wrench_topic_ = _sdf->GetElement("wrenchTopic")->Get(); + if (_sdf->HasElement("supplyTopic")) + supply_topic_ = _sdf->GetElement("supplyTopic")->Get(); + if (_sdf->HasElement("statusTopic")) + status_topic_ = _sdf->GetElement("statusTopic")->Get(); + if (_sdf->HasElement("frameId")) + frame_id_ = _sdf->GetElement("frameId")->Get(); + + if (_sdf->HasElement("voltageTopicName")) + { + gzwarn << "[quadrotor_propulsion] Plugin parameter 'voltageTopicName' is deprecated! Plese change your config to use " + << "'topicName' for MotorCommand messages or 'pwmTopicName' for MotorPWM messages." << std::endl; + pwm_topic_ = _sdf->GetElement("voltageTopicName")->Get(); + } + + // set control timing parameters + controlTimer.Load(world, _sdf, "control"); + motorStatusTimer.Load(world, _sdf, "motorStatus"); + if (_sdf->HasElement("controlTolerance")) + control_tolerance_.fromSec(_sdf->GetElement("controlTolerance")->Get()); + if (_sdf->HasElement("controlDelay")) + control_delay_.fromSec(_sdf->GetElement("controlDelay")->Get()); + + // set initial supply voltage + if (_sdf->HasElement("supplyVoltage")) + model_.setInitialSupplyVoltage(_sdf->GetElement("supplyVoltage")->Get()); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + node_handle_ = new ros::NodeHandle(namespace_); + + // get model parameters + if (!model_.configure(ros::NodeHandle(*node_handle_, param_namespace_))) + { + gzwarn << "[quadrotor_propulsion] Could not properly configure the propulsion plugin. Make sure you loaded the parameter file." << std::endl; + return; + } + + // publish trigger + if (!trigger_topic_.empty()) + { + ros::AdvertiseOptions ops; + ops.callback_queue = &callback_queue_; + ops.init(trigger_topic_, 10); + trigger_publisher_ = node_handle_->advertise(ops); + } + + // subscribe voltage command + if (!command_topic_.empty()) + { + ros::SubscribeOptions ops; + ops.callback_queue = &callback_queue_; + ops.init(command_topic_, 1, boost::bind(&QuadrotorPropulsion::addCommandToQueue, &model_, _1)); + command_subscriber_ = node_handle_->subscribe(ops); + } + + // subscribe pwm command + if (!pwm_topic_.empty()) + { + ros::SubscribeOptions ops; + ops.callback_queue = &callback_queue_; + ops.init(pwm_topic_, 1, boost::bind(&QuadrotorPropulsion::addPWMToQueue, &model_, _1)); + pwm_subscriber_ = node_handle_->subscribe(ops); + } + + // advertise wrench + if (!wrench_topic_.empty()) + { + ros::AdvertiseOptions ops; + ops.callback_queue = &callback_queue_; + ops.init(wrench_topic_, 10); + wrench_publisher_ = node_handle_->advertise(ops); + } + + // advertise and latch supply + if (!supply_topic_.empty()) + { + ros::AdvertiseOptions ops; + ops.callback_queue = &callback_queue_; + ops.latch = true; + ops.init(supply_topic_, 10); + supply_publisher_ = node_handle_->advertise(ops); + supply_publisher_.publish(model_.getSupply()); + } + + // advertise motor_status + if (!status_topic_.empty()) + { + ros::AdvertiseOptions ops; + ops.callback_queue = &callback_queue_; + ops.init(status_topic_, 10); + motor_status_publisher_ = node_handle_->advertise(ops); + } + + // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorPropulsion::QueueThread,this ) ); + + Reset(); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboQuadrotorPropulsion::Update, this)); +} + +// #ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 +// double GazeboQuadrotorPropulsion_Update = 0.0; +// int num1 = 0; +// #endif +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboQuadrotorPropulsion::Update() +{ +// #ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19 +// struct timeval tv; +// double start_time, cur_time, tmp_time; +// gettimeofday(&tv, NULL); +// start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +// #endif + + std::cout<<"==========================GazeboQuadrotorPropulsion::Update========================"<GetSimTime(); + Time dt = current_time - last_time_; + last_time_ = current_time; + if (dt <= 0.0) + return; + + // Send trigger + bool trigger = controlTimer.getUpdatePeriod() > 0.0 ? controlTimer.update() : false; + if (trigger && trigger_publisher_) + { + rosgraph_msgs::Clock clock; + clock.clock = ros::Time(current_time.sec, current_time.nsec); + trigger_publisher_.publish(clock); + + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Sent a trigger message at t = " << current_time.Double() << " (dt = " << (current_time - last_trigger_time_).Double() << ")"); + last_trigger_time_ = current_time; + } + + // Get new commands/state + callback_queue_.callAvailable(); + + // Process input queue + model_.processQueue(ros::Time(current_time.sec, current_time.nsec), control_tolerance_, control_delay_, (model_.getMotorStatus().on && trigger) ? ros::WallDuration(1.0) : ros::WallDuration(), &callback_queue_); + + // fill input vector u for propulsion model + geometry_msgs::Twist twist; + fromVector(link->GetRelativeLinearVel(), twist.linear); + fromVector(link->GetRelativeAngularVel(), twist.angular); + model_.setTwist(twist); + + // update the model + model_.update(dt.Double()); + + // get wrench from model + Vector3 force, torque; + toVector(model_.getWrench().force, force); + toVector(model_.getWrench().torque, torque); + + // publish wrench + if (wrench_publisher_) + { + geometry_msgs::WrenchStamped wrench_msg; + wrench_msg.header.stamp = ros::Time(current_time.sec, current_time.nsec); + wrench_msg.header.frame_id = frame_id_; + wrench_msg.wrench = model_.getWrench(); + wrench_publisher_.publish(wrench_msg); + } + + // publish motor status + if (motor_status_publisher_ && motorStatusTimer.update() /* && current_time >= last_motor_status_time_ + control_period_ */) + { + hector_uav_msgs::MotorStatus motor_status = model_.getMotorStatus(); + motor_status.header.stamp = ros::Time(current_time.sec, current_time.nsec); + motor_status_publisher_.publish(motor_status); + last_motor_status_time_ = current_time; + } + + // publish supply + if (supply_publisher_ && current_time >= last_supply_time_ + 1.0) + { + supply_publisher_.publish(model_.getSupply()); + last_supply_time_ = current_time; + } + + // set force and torque in gazebo + link->AddRelativeForce(force); + link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force)); + +// #ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19 +// gettimeofday(&tv, NULL); +// GazeboQuadrotorPropulsion_Update += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; +// num1++; +// if (num1 == 199999) +// { +// std::cout << "GazeboQuadrotorPropulsion_Update: " << GazeboQuadrotorPropulsion_Update << "num:" << num1 << std::endl; +// } +// #endif +} + +//////////////////////////////////////////////////////////////////////////////// +// Reset the controller +void GazeboQuadrotorPropulsion::Reset() +{ + model_.reset(); + last_time_ = Time(); + last_trigger_time_ = Time(); + last_motor_status_time_ = Time(); + last_supply_time_ = Time(); +} + +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboQuadrotorPropulsion::QueueThread() +{ + static const double timeout = 0.01; + + while (node_handle_->ok()) + { + callback_queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorPropulsion) + +} // namespace gazebo diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_simple_controller.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_simple_controller.cpp new file mode 100644 index 0000000..13c8cea --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_quadrotor_simple_controller.cpp @@ -0,0 +1,426 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include +#include + +#include + +#include + +namespace gazebo { + +GazeboQuadrotorSimpleController::GazeboQuadrotorSimpleController() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboQuadrotorSimpleController::~GazeboQuadrotorSimpleController() +{ + event::Events::DisconnectWorldUpdateBegin(updateConnection); + + node_handle_->shutdown(); + delete node_handle_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + gzwarn << "The GazeboQuadrotorSimpleController plugin is DEPRECATED in ROS hydro. Please use the twist_controller in package hector_quadrotor_controller instead." << std::endl; + + world = _model->GetWorld(); + link = _model->GetLink(); + link_name_ = link->GetName(); + + // default parameters + namespace_.clear(); + velocity_topic_ = "cmd_vel"; + imu_topic_.clear(); + state_topic_.clear(); + wrench_topic_ = "wrench_out"; + max_force_ = -1; + auto_engage_ = true; + + // load parameters from sdf + if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get(); + if (_sdf->HasElement("topicName")) velocity_topic_ = _sdf->GetElement("topicName")->Get(); + if (_sdf->HasElement("imuTopic")) imu_topic_ = _sdf->GetElement("imuTopic")->Get(); + if (_sdf->HasElement("stateTopic")) state_topic_ = _sdf->GetElement("stateTopic")->Get(); + if (_sdf->HasElement("wrenchTopic")) wrench_topic_ = _sdf->GetElement("wrenchTopic")->Get(); + if (_sdf->HasElement("maxForce")) max_force_ = _sdf->GetElement("maxForce")->Get(); + if (_sdf->HasElement("autoEngage")) auto_engage_ = _sdf->GetElement("autoEngage")->Get(); + + if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue()) { + link_name_ = _sdf->GetElement("bodyName")->Get(); + link = _model->GetLink(link_name_); + } + + if (!link) + { + ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str()); + return; + } + + // configure controllers + controllers_.roll.Load(_sdf, "rollpitch"); + controllers_.pitch.Load(_sdf, "rollpitch"); + controllers_.yaw.Load(_sdf, "yaw"); + controllers_.velocity_x.Load(_sdf, "velocityXY"); + controllers_.velocity_y.Load(_sdf, "velocityXY"); + controllers_.velocity_z.Load(_sdf, "velocityZ"); + + // Get inertia and mass of quadrotor body + inertia = link->GetInertial()->GetPrincipalMoments(); + mass = link->GetInertial()->GetMass(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + node_handle_ = new ros::NodeHandle(namespace_); + ros::NodeHandle param_handle(*node_handle_, "controller"); + + // subscribe command + param_handle.getParam("velocity_topic", velocity_topic_); + if (!velocity_topic_.empty()) + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create( + velocity_topic_, 1, + boost::bind(&GazeboQuadrotorSimpleController::VelocityCallback, this, _1), + ros::VoidPtr(), &callback_queue_); + velocity_subscriber_ = node_handle_->subscribe(ops); + } + + // subscribe imu + param_handle.getParam("imu_topic", imu_topic_); + if (!imu_topic_.empty()) + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create( + imu_topic_, 1, + boost::bind(&GazeboQuadrotorSimpleController::ImuCallback, this, _1), + ros::VoidPtr(), &callback_queue_); + imu_subscriber_ = node_handle_->subscribe(ops); + + ROS_INFO_NAMED("quadrotor_simple_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str()); + } + + // subscribe state + param_handle.getParam("state_topic", state_topic_); + if (!state_topic_.empty()) + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create( + state_topic_, 1, + boost::bind(&GazeboQuadrotorSimpleController::StateCallback, this, _1), + ros::VoidPtr(), &callback_queue_); + state_subscriber_ = node_handle_->subscribe(ops); + + ROS_INFO_NAMED("quadrotor_simple_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str()); + } + + // advertise wrench + param_handle.getParam("wrench_topic", wrench_topic_); + if (!wrench_topic_.empty()) + { + ros::AdvertiseOptions ops = ros::AdvertiseOptions::create( + wrench_topic_, 10, + ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_ + ); + wrench_publisher_ = node_handle_->advertise(ops); + } + + // engage/shutdown service servers + { + ros::AdvertiseServiceOptions ops = ros::AdvertiseServiceOptions::create( + "engage", boost::bind(&GazeboQuadrotorSimpleController::EngageCallback, this, _1, _2), + ros::VoidConstPtr(), &callback_queue_ + ); + engage_service_server_ = node_handle_->advertiseService(ops); + + ops = ros::AdvertiseServiceOptions::create( + "shutdown", boost::bind(&GazeboQuadrotorSimpleController::ShutdownCallback, this, _1, _2), + ros::VoidConstPtr(), &callback_queue_ + ); + shutdown_service_server_ = node_handle_->advertiseService(ops); + } + + // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorSimpleController::CallbackQueueThread,this ) ); + + + Reset(); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + controlTimer.Load(world, _sdf); + updateConnection = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboQuadrotorSimpleController::Update, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Callbacks +void GazeboQuadrotorSimpleController::VelocityCallback(const geometry_msgs::TwistConstPtr& velocity) +{ + velocity_command_ = *velocity; +} + +void GazeboQuadrotorSimpleController::ImuCallback(const sensor_msgs::ImuConstPtr& imu) +{ + pose.rot.Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z); + euler = pose.rot.GetAsEuler(); + angular_velocity = pose.rot.RotateVector(math::Vector3(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z)); +} + +void GazeboQuadrotorSimpleController::StateCallback(const nav_msgs::OdometryConstPtr& state) +{ + math::Vector3 velocity1(velocity); + + if (imu_topic_.empty()) { + pose.pos.Set(state->pose.pose.position.x, state->pose.pose.position.y, state->pose.pose.position.z); + pose.rot.Set(state->pose.pose.orientation.w, state->pose.pose.orientation.x, state->pose.pose.orientation.y, state->pose.pose.orientation.z); + euler = pose.rot.GetAsEuler(); + angular_velocity.Set(state->twist.twist.angular.x, state->twist.twist.angular.y, state->twist.twist.angular.z); + } + + velocity.Set(state->twist.twist.linear.x, state->twist.twist.linear.y, state->twist.twist.linear.z); + + // calculate acceleration + double dt = !state_stamp.isZero() ? (state->header.stamp - state_stamp).toSec() : 0.0; + state_stamp = state->header.stamp; + if (dt > 0.0) { + acceleration = (velocity - velocity1) / dt; + } else { + acceleration.Set(); + } +} + +bool GazeboQuadrotorSimpleController::EngageCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &) +{ + ROS_INFO_NAMED("quadrotor_simple_controller", "Engaging motors!"); + running_ = true; + return true; +} + +bool GazeboQuadrotorSimpleController::ShutdownCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &) +{ + ROS_INFO_NAMED("quadrotor_simple_controller", "Shutting down motors!"); + running_ = false; + return true; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboQuadrotorSimpleController::Update() +{ + std::cout<<"=========================GazeboQuadrotorSimpleController::Update========================="< 0.0) { + // Get Pose/Orientation from Gazebo (if no state subscriber is active) + if (imu_topic_.empty()) { + pose = link->GetWorldPose(); + angular_velocity = link->GetWorldAngularVel(); + euler = pose.rot.GetAsEuler(); + } + if (state_topic_.empty()) { + acceleration = (link->GetWorldLinearVel() - velocity) / dt; + velocity = link->GetWorldLinearVel(); + } + + // Auto engage/shutdown + if (auto_engage_) { + if (!running_ && velocity_command_.linear.z > 0.1) { + running_ = true; + ROS_INFO_NAMED("quadrotor_simple_controller", "Engaging motors!"); + } else if (running_ && controllers_.velocity_z.i < -1.0 && velocity_command_.linear.z < -0.1 && (velocity.z > -0.1 && velocity.z < 0.1)) { + running_ = false; + ROS_INFO_NAMED("quadrotor_simple_controller", "Shutting down motors!"); + } + } + + // static Time lastDebug; + // if ((world->GetSimTime() - lastDebug).Double() > 0.5) { + // ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Velocity: gazebo = [" << link->GetWorldLinearVel() << "], state = [" << velocity << "]"); + // ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Acceleration: gazebo = [" << link->GetWorldLinearAccel() << "], state = [" << acceleration << "]"); + // ROS_DEBUG_STREAM_NAMED("quadrotor_simple_controller", "Angular Velocity: gazebo = [" << link->GetWorldAngularVel() << "], state = [" << angular_velocity << "]"); + // lastDebug = world->GetSimTime(); + // } + + // Get gravity + math::Vector3 gravity_body = pose.rot.RotateVector(world->GetPhysicsEngine()->GetGravity()); + double gravity = gravity_body.GetLength(); + double load_factor = gravity * gravity / world->GetPhysicsEngine()->GetGravity().Dot(gravity_body); // Get gravity + + // Rotate vectors to coordinate frames relevant for control + math::Quaternion heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2)); + math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity); + math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration); + math::Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity); + + // update controllers + force.Set(0.0, 0.0, 0.0); + torque.Set(0.0, 0.0, 0.0); + if (running_) { + double pitch_command = controllers_.velocity_x.update(velocity_command_.linear.x, velocity_xy.x, acceleration_xy.x, dt) / gravity; + double roll_command = -controllers_.velocity_y.update(velocity_command_.linear.y, velocity_xy.y, acceleration_xy.y, dt) / gravity; + torque.x = inertia.x * controllers_.roll.update(roll_command, euler.x, angular_velocity_body.x, dt); + torque.y = inertia.y * controllers_.pitch.update(pitch_command, euler.y, angular_velocity_body.y, dt); + // torque.x = inertia.x * controllers_.roll.update(-velocity_command_.linear.y/gravity, euler.x, angular_velocity_body.x, dt); + // torque.y = inertia.y * controllers_.pitch.update(velocity_command_.linear.x/gravity, euler.y, angular_velocity_body.y, dt); + torque.z = inertia.z * controllers_.yaw.update(velocity_command_.angular.z, angular_velocity.z, 0, dt); + force.z = mass * (controllers_.velocity_z.update(velocity_command_.linear.z, velocity.z, acceleration.z, dt) + load_factor * gravity); + if (max_force_ > 0.0 && force.z > max_force_) force.z = max_force_; + if (force.z < 0.0) force.z = 0.0; + + } else { + controllers_.roll.reset(); + controllers_.pitch.reset(); + controllers_.yaw.reset(); + controllers_.velocity_x.reset(); + controllers_.velocity_y.reset(); + controllers_.velocity_z.reset(); + } + + // static double lastDebugOutput = 0.0; + // if (last_time.Double() - lastDebugOutput > 0.1) { + // ROS_DEBUG_NAMED("quadrotor_simple_controller", "Velocity = [%g %g %g], Acceleration = [%g %g %g]", velocity.x, velocity.y, velocity.z, acceleration.x, acceleration.y, acceleration.z); + // ROS_DEBUG_NAMED("quadrotor_simple_controller", "Command: linear = [%g %g %g], angular = [%g %g %g], roll/pitch = [%g %g]", velocity_command_.linear.x, velocity_command_.linear.y, velocity_command_.linear.z, velocity_command_.angular.x*180/M_PI, velocity_command_.angular.y*180/M_PI, velocity_command_.angular.z*180/M_PI, roll_command*180/M_PI, pitch_command*180/M_PI); + // ROS_DEBUG_NAMED("quadrotor_simple_controller", "Mass: %g kg, Inertia: [%g %g %g], Load: %g g", mass, inertia.x, inertia.y, inertia.z, load_factor); + // ROS_DEBUG_NAMED("quadrotor_simple_controller", "Force: [%g %g %g], Torque: [%g %g %g]", force.x, force.y, force.z, torque.x, torque.y, torque.z); + // lastDebugOutput = last_time.Double(); + // } + + // Publish wrench + if (wrench_publisher_) { + geometry_msgs::Wrench wrench; + wrench.force.x = force.x; + wrench.force.y = force.y; + wrench.force.z = force.z; + wrench.torque.x = torque.x; + wrench.torque.y = torque.y; + wrench.torque.z = torque.z; + wrench_publisher_.publish(wrench); + } + } + + // set force and torque in gazebo + link->AddRelativeForce(force); + link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Reset the controller +void GazeboQuadrotorSimpleController::Reset() +{ + controllers_.roll.reset(); + controllers_.pitch.reset(); + controllers_.yaw.reset(); + controllers_.velocity_x.reset(); + controllers_.velocity_y.reset(); + controllers_.velocity_z.reset(); + + force.Set(); + torque.Set(); + + // reset state + pose.Reset(); + velocity.Set(); + angular_velocity.Set(); + acceleration.Set(); + euler.Set(); + state_stamp = ros::Time(); + + running_ = false; +} + +//////////////////////////////////////////////////////////////////////////////// +// PID controller implementation +GazeboQuadrotorSimpleController::PIDController::PIDController() +{ +} + +GazeboQuadrotorSimpleController::PIDController::~PIDController() +{ +} + +void GazeboQuadrotorSimpleController::PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix) +{ + gain_p = 0.0; + gain_d = 0.0; + gain_i = 0.0; + time_constant = 0.0; + limit = -1.0; + + if (!_sdf) return; + // _sdf->PrintDescription(_sdf->GetName()); + if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->Get(); + if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->Get(); + if (_sdf->HasElement(prefix + "IntegralGain")) gain_i = _sdf->GetElement(prefix + "IntegralGain")->Get(); + if (_sdf->HasElement(prefix + "TimeConstant")) time_constant = _sdf->GetElement(prefix + "TimeConstant")->Get(); + if (_sdf->HasElement(prefix + "Limit")) limit = _sdf->GetElement(prefix + "Limit")->Get(); +} + +double GazeboQuadrotorSimpleController::PIDController::update(double new_input, double x, double dx, double dt) +{ + // limit command + if (limit > 0.0 && fabs(new_input) > limit) new_input = (new_input < 0 ? -1.0 : 1.0) * limit; + + // filter command + if (dt + time_constant > 0.0) { + dinput = (new_input - input) / (dt + time_constant); + input = (dt * new_input + time_constant * input) / (dt + time_constant); + } + + // update proportional, differential and integral errors + p = input - x; + d = dinput - dx; + i = i + dt * p; + + // update control output + output = gain_p * p + gain_d * d + gain_i * i; + return output; +} + +void GazeboQuadrotorSimpleController::PIDController::reset() +{ + input = dinput = 0; + p = i = d = output = 0; +} + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorSimpleController) + +} // namespace gazebo diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_ros_baro.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_ros_baro.cpp new file mode 100644 index 0000000..f6ed76c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/src/gazebo_ros_baro.cpp @@ -0,0 +1,160 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include +#include + +static const double DEFAULT_ELEVATION = 0.0; +static const double DEFAULT_QNH = 1013.25; + +namespace gazebo { + +GazeboRosBaro::GazeboRosBaro() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosBaro::~GazeboRosBaro() +{ + updateTimer.Disconnect(updateConnection); + + dynamic_reconfigure_server_.reset(); + + node_handle_->shutdown(); + delete node_handle_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + world = _model->GetWorld(); + link = _model->GetLink(); + link_name_ = link->GetName(); + + if (_sdf->HasElement("bodyName")) + { + link_name_ = _sdf->GetElement("bodyName")->Get(); + link = _model->GetLink(link_name_); + } + + if (!link) + { + ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str()); + return; + } + + // default parameters + namespace_.clear(); + frame_id_ = link->GetName(); + height_topic_ = "pressure_height"; + altimeter_topic_ = "altimeter"; + elevation_ = DEFAULT_ELEVATION; + qnh_ = DEFAULT_QNH; + + // load parameters + if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get(); + if (_sdf->HasElement("frameId")) frame_id_ = _sdf->GetElement("frameId")->Get(); + if (_sdf->HasElement("topicName")) height_topic_ = _sdf->GetElement("topicName")->Get(); + if (_sdf->HasElement("altimeterTopicName")) altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->Get(); + if (_sdf->HasElement("elevation")) elevation_ = _sdf->GetElement("elevation")->Get(); + if (_sdf->HasElement("qnh")) qnh_ = _sdf->GetElement("qnh")->Get(); + + // load sensor model + sensor_model_.Load(_sdf); + + height_.header.frame_id = frame_id_; + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + node_handle_ = new ros::NodeHandle(namespace_); + + // advertise height + if (!height_topic_.empty()) { + height_publisher_ = node_handle_->advertise(height_topic_, 10); + } + + // advertise altimeter + if (!altimeter_topic_.empty()) { + altimeter_publisher_ = node_handle_->advertise(altimeter_topic_, 10); + } + + // setup dynamic_reconfigure server + dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server(ros::NodeHandle(*node_handle_, altimeter_topic_))); + dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel::dynamicReconfigureCallback, &sensor_model_, _1, _2)); + + Reset(); + + // connect Update function + updateTimer.Load(world, _sdf); + updateConnection = updateTimer.Connect(boost::bind(&GazeboRosBaro::Update, this)); +} + +void GazeboRosBaro::Reset() +{ + updateTimer.Reset(); + sensor_model_.reset(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosBaro::Update() +{ + common::Time sim_time = world->GetSimTime(); + double dt = updateTimer.getTimeSinceLastUpdate().Double(); + + math::Pose pose = link->GetWorldPose(); + double height = sensor_model_(pose.pos.z, dt); + + if (height_publisher_) { + height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec); + height_.point.z = height; + height_publisher_.publish(height_); + } + + if (altimeter_publisher_) { + altimeter_.header = height_.header; + altimeter_.altitude = height + elevation_; + altimeter_.pressure = pow((1.0 - altimeter_.altitude / 44330.0), 5.263157) * qnh_; + altimeter_.qnh = qnh_; + altimeter_publisher_.publish(altimeter_); + } +} + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboRosBaro) + +} // namespace gazebo diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/.gitignore b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/.gitignore new file mode 100644 index 0000000..ffe2a80 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/.gitignore @@ -0,0 +1 @@ +matlab/codegen/ diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/CHANGELOG.rst new file mode 100644 index 0000000..a4b55c0 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/CHANGELOG.rst @@ -0,0 +1,48 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ +* make models more robust against irregular input values + Especially on collisions with walls and obstacles Gazebo can output very high + velocity values for some reason, which caused the propulsion and aerodynamic + models to output infinite values or NaN as resulting forces and torques, with + the consequence that Gazebo effectively stopped simulation and showed the quadrotor + at the origin of the world frame. + With this patch the simulation is much more stable in case of collisions or + hard landings. +* disabled quadratic term (CT2s) in propeller speed to performance factor J + Because of this term the model output a non-zero thrust even if the propeller speed was zero. + The quadratic term is due to drag and therefore covered in the aerodynamics model. +* Contributors: Johannes Meyer + +0.3.3 (2014-09-01) +------------------ +* fixed target_link_libraries() line for the quadrotor_aerodynamics library +* fixed missing cmake commands to link with boost and roscpp +* always use newest available motor command in propulsion plugin +* added cmake_modules dependency +* Contributors: Johannes Meyer, Rasit Eskicioglu + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ +* fixed copy&paste error in quadrotor_aerodynamics.cpp +* output drag forces in body coordinate system and added orientation input +* fixed configuration namespace and plugin cleanup +* added boolean return value for configure() methods +* accept hector_uav_msgs/MotorCommand messages directly for the propulsion model/plugin +* copied generated C code within the cpp classes to not require Matlab for compilation +* fixed sign of aerodynamic forces +* use precompiled codegen libraries if available +* Contributors: Johannes Meyer + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/CMakeLists.txt new file mode 100644 index 0000000..a5cab85 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/CMakeLists.txt @@ -0,0 +1,72 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_model) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS geometry_msgs hector_uav_msgs roscpp cmake_modules) +include_directories(include ${catkin_INCLUDE_DIRS}) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS thread) +include_directories(${Boost_INCLUDE_DIRS}) + +find_package(Eigen REQUIRED) +include_directories(${EIGEN_INCLUDE_DIRS}) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES hector_quadrotor_propulsion hector_quadrotor_aerodynamics + CATKIN_DEPENDS geometry_msgs hector_uav_msgs roscpp + DEPENDS Boost +) + +########### +## Build ## +########### + +add_library(hector_quadrotor_propulsion src/quadrotor_propulsion.cpp) +add_dependencies(hector_quadrotor_propulsion hector_uav_msgs_generate_messages_cpp) +target_link_libraries(hector_quadrotor_propulsion ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +#target_link_libraries(hector_quadrotor_propulsion quadrotorPropulsion) + +add_library(hector_quadrotor_aerodynamics src/quadrotor_aerodynamics.cpp) +target_link_libraries(hector_quadrotor_aerodynamics ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +#target_link_libraries(hector_quadrotor_aerodynamics quadrotorDrag) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +install( + TARGETS hector_quadrotor_propulsion hector_quadrotor_aerodynamics + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY param DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/helpers.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/helpers.h new file mode 100644 index 0000000..bc817ee --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/helpers.h @@ -0,0 +1,177 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer and contributors, Technische Universitat Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_MODEL_HELPERS_H +#define HECTOR_QUADROTOR_MODEL_HELPERS_H + +#include +#include + +namespace hector_quadrotor_model +{ + +template int isnan(const T& value) { + return std::isnan(value); +} + +template int isnan(const boost::array& array) { + for(typename boost::array::const_iterator it = array.begin(); it != array.end(); ++it) + if (std::isnan(*it)) return std::isnan(*it); + return false; +} + +template int isnan(const boost::iterator_range& range, const typename boost::iterator_range::value_type& min, const typename boost::iterator_range::value_type& max) { + for(IteratorT it = range.begin(); it != range.end(); ++it) + if (std::isnan(*it)) return std::isnan(*it); + return false; +} + +template int isinf(const T& value) { + return std::isinf(value); +} + +template int isinf(const boost::array& array) { + for(typename boost::array::const_iterator it = array.begin(); it != array.end(); ++it) + if (std::isinf(*it)) return std::isinf(*it); + return false; +} + +template int isinf(const boost::iterator_range& range, const typename boost::iterator_range::value_type& min, const typename boost::iterator_range::value_type& max) { + for(IteratorT it = range.begin(); it != range.end(); ++it) + if (std::isinf(*it)) return std::isinf(*it); + return false; +} + +template void limit(T& value, const T& min, const T& max) { + if (!isnan(min) && value < min) value = min; + if (!isnan(max) && value > max) value = max; +} + +template void limit(boost::array& array, const T& min, const T& max) { + for(typename boost::array::iterator it = array.begin(); it != array.end(); ++it) + limit(*it, min, max); +} + +template void limit(const boost::iterator_range& range, const typename boost::iterator_range::value_type& min, const typename boost::iterator_range::value_type& max) { + for(IteratorT it = range.begin(); it != range.end(); ++it) + limit(*it, min, max); +} + +template static inline void checknan(T& value, const std::string& text = "") { + if (isnan(value)) { +#ifndef NDEBUG + if (!text.empty()) std::cerr << text << " contains **!?* Nan values!" << std::endl; +#endif + value = T(); + return; + } + + if (isinf(value)) { +#ifndef NDEBUG + if (!text.empty()) std::cerr << text << " is +-Inf!" << std::endl; +#endif + value = T(); + return; + } +} + +template static inline void toVector(const Message& msg, Vector& vector) +{ + vector.x = msg.x; + vector.y = msg.y; + vector.z = msg.z; +} + +template static inline void fromVector(const Vector& vector, Message& msg) +{ + msg.x = vector.x; + msg.y = vector.y; + msg.z = vector.z; +} + +template static inline void toQuaternion(const Message& msg, Quaternion& vector) +{ + vector.w = msg.w; + vector.x = msg.x; + vector.y = msg.y; + vector.z = msg.z; +} + +template static inline void fromQuaternion(const Quaternion& vector, Message& msg) +{ + msg.w = vector.w; + msg.x = vector.x; + msg.y = vector.y; + msg.z = vector.z; +} + +static inline geometry_msgs::Vector3 operator+(const geometry_msgs::Vector3& a, const geometry_msgs::Vector3& b) +{ + geometry_msgs::Vector3 result; + result.x = a.x + b.x; + result.y = a.y + b.y; + result.z = a.z + b.z; + return result; +} + +static inline geometry_msgs::Wrench operator+(const geometry_msgs::Wrench& a, const geometry_msgs::Wrench& b) +{ + geometry_msgs::Wrench result; + result.force = a.force + b.force; + result.torque = a.torque + b.torque; + return result; +} + +template +class PrintVector { +public: + typedef const T* const_iterator; + PrintVector(const_iterator begin, const_iterator end, const std::string &delimiter = "[ ]") : begin_(begin), end_(end), delimiter_(delimiter) {} + const_iterator begin() const { return begin_; } + const_iterator end() const { return end_; } + std::size_t size() const { return end_ - begin_; } + + std::ostream& operator>>(std::ostream& os) const { + if (!delimiter_.empty()) os << delimiter_.substr(0, delimiter_.size() - 1); + for(const_iterator it = begin(); it != end(); ++it) { + if (it != begin()) os << " "; + os << *it; + } + if (!delimiter_.empty()) os << delimiter_.substr(1, delimiter_.size() - 1); + return os; + } + +private: + const_iterator begin_, end_; + std::string delimiter_; +}; +template std::ostream &operator<<(std::ostream& os, const PrintVector& vector) { return vector >> os; } + +} + +#endif // HECTOR_QUADROTOR_MODEL_HELPERS_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_aerodynamics.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_aerodynamics.h new file mode 100644 index 0000000..0ad8d18 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_aerodynamics.h @@ -0,0 +1,77 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_MODEL_QUADROTOR_AERODYNAMICS_H +#define HECTOR_QUADROTOR_MODEL_QUADROTOR_AERODYNAMICS_H + +#include +#include +#include +#include + +#include + +#include + +namespace hector_quadrotor_model +{ + +class QuadrotorAerodynamics { +public: + QuadrotorAerodynamics(); + ~QuadrotorAerodynamics(); + + bool configure(const ros::NodeHandle ¶m = ros::NodeHandle("~")); + void reset(); + void update(double dt); + + void setOrientation(const geometry_msgs::Quaternion& orientation); + void setTwist(const geometry_msgs::Twist& twist); + void setBodyTwist(const geometry_msgs::Twist& twist); + void setWind(const geometry_msgs::Vector3& wind); + + const geometry_msgs::Wrench& getWrench() const { return wrench_; } + + void f(const double uin[6], double dt, double y[6]) const; + +private: + geometry_msgs::Quaternion orientation_; + geometry_msgs::Twist twist_; + geometry_msgs::Vector3 wind_; + + geometry_msgs::Wrench wrench_; + + boost::mutex mutex_; + + class DragModel; + DragModel *drag_model_; +}; + +} + +#endif // HECTOR_QUADROTOR_MODEL_QUADROTOR_AERODYNAMICS_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_propulsion.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_propulsion.h new file mode 100644 index 0000000..d103847 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include/hector_quadrotor_model/quadrotor_propulsion.h @@ -0,0 +1,99 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_MODEL_QUADROTOR_PROPULSION_H +#define HECTOR_QUADROTOR_MODEL_QUADROTOR_PROPULSION_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace hector_quadrotor_model +{ + +class QuadrotorPropulsion +{ +public: + QuadrotorPropulsion(); + ~QuadrotorPropulsion(); + + bool configure(const ros::NodeHandle ¶m = ros::NodeHandle("~")); + void reset(); + void update(double dt); + + void engage(); + void shutdown(); + + void setTwist(const geometry_msgs::Twist& twist); + void setVoltage(const hector_uav_msgs::MotorPWM& command); + + const geometry_msgs::Wrench& getWrench() const { return wrench_; } + const hector_uav_msgs::Supply& getSupply() const { return supply_; } + const hector_uav_msgs::MotorStatus& getMotorStatus() const { return motor_status_; } + + void addCommandToQueue(const hector_uav_msgs::MotorCommandConstPtr& command); + void addPWMToQueue(const hector_uav_msgs::MotorPWMConstPtr& pwm); + bool processQueue(const ros::Time& timestamp, const ros::Duration& tolerance = ros::Duration(), const ros::Duration& delay = ros::Duration(), const ros::WallDuration &wait = ros::WallDuration(), ros::CallbackQueue *callback_queue = 0); + + void f(const double xin[4], const double uin[10], double dt, double y[14], double xpred[4]) const; + + void setInitialSupplyVoltage(double voltage) { initial_voltage_ = voltage; } + +private: + geometry_msgs::Wrench wrench_; + hector_uav_msgs::Supply supply_; + hector_uav_msgs::MotorStatus motor_status_; + ros::Time last_command_time_; + + double initial_voltage_; + + std::queue command_queue_; + boost::mutex command_queue_mutex_; + boost::condition command_condition_; + + boost::mutex mutex_; + + class PropulsionModel; + PropulsionModel *propulsion_model_; +}; + +} + +#endif // HECTOR_QUADROTOR_MODEL_QUADROTOR_PROPULSION_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/CMakeLists.txt new file mode 100644 index 0000000..b2a0b47 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/CMakeLists.txt @@ -0,0 +1,60 @@ +find_program(MATLAB NAMES matlab) + +if(MATLAB) + message(STATUS "Using Matlab at ${MATLAB}") + file(GLOB MFILES "*.m") + + include_directories(codegen/lib) + + add_custom_command( + OUTPUT codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a codegen/lib/quadrotorDrag/quadrotorDrag.a + COMMAND ${MATLAB} -nojvm -nodesktop -nosplash -r "compile_all; quit;" + DEPENDS ${MFILES} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) + +# add_library(quadrotorPropulsion STATIC IMPORTED GLOBAL) +# set_target_properties(quadrotorPropulsion PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) + + # workaround for cmake <2.8.8 + add_library(quadrotorPropulsion STATIC dummy.c) + target_link_libraries(quadrotorPropulsion ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) + +# add_library(quadrotorDrag STATIC IMPORTED GLOBAL) +# set_target_properties(quadrotorDrag PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) + + # workaround for cmake <2.8.8 + add_library(quadrotorDrag STATIC dummy.c) + target_link_libraries(quadrotorDrag ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) + +else() + if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) + message(STATUS "Found precompiled hector_quadrotor propulsion model.") + +# add_library(quadrotorPropulsion STATIC IMPORTED GLOBAL) +# set_target_properties(quadrotorPropulsion PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) + + # workaround for cmake <2.8.8 + add_library(quadrotorPropulsion STATIC dummy.c) + target_link_libraries(quadrotorPropulsion ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorPropulsion/quadrotorPropulsion.a) + + endif() + + if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) + message(STATUS "Found precompiled hector_quadrotor drag model.") + +# add_library(quadrotorDrag STATIC IMPORTED GLOBAL) +# set_target_properties(quadrotorDrag PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) + + # workaround for cmake <2.8.8 + add_library(quadrotorDrag STATIC dummy.c) + target_link_libraries(quadrotorDrag ${CMAKE_CURRENT_SOURCE_DIR}/codegen/lib/quadrotorDrag/quadrotorDrag.a) + endif() + + if(NOT TARGET quadrotorPropulsion OR NOT TARGET quadrotorDrag) + message(WARNING "Matlab not found. Cannot build hector_quadrotor_model libraries.") + endif() +endif(MATLAB) + +set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES ${CMAKE_CURRENT_SOURCE_DIR}/codegen) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/compile_all.m b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/compile_all.m new file mode 100644 index 0000000..c771b06 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/compile_all.m @@ -0,0 +1,8 @@ +rtw_config = coder.config('LIB'); +rtw_config.TargetLang = 'C'; +rtw_config.CodeReplacementLibrary = 'C89/C90 (ANSI)'; +rtw_config.CCompilerOptimization = 'On'; + +codegen -config rtw_config -v quadrotorPropulsion motorspeed -o quadrotorPropulsion +codegen -config rtw_config -v quadrotorDrag -o quadrotorDrag + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/dummy.c b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/dummy.c new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/dummy.c @@ -0,0 +1 @@ + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/motorspeed.m b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/motorspeed.m new file mode 100644 index 0000000..2bc3f66 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/motorspeed.m @@ -0,0 +1,71 @@ +function [M_e,I,xpred] = motorspeed(xin, uin, parameter, dt) +%#eml + +assert(isa(xin,'double')); +assert(all(size(xin) == 1)); + +assert(isa(uin,'double')); +assert(all(size(uin) == [2 1])); + +assert(isa(parameter,'struct')); +assert(isa(parameter.k_m,'double')); +assert(isa(parameter.k_t,'double')); + +assert(isa(parameter.CT2s,'double')); +assert(isa(parameter.CT1s,'double')); +assert(isa(parameter.CT0s,'double')); + +assert(isa(parameter.Psi,'double')); +assert(isa(parameter.J_M,'double')); +assert(isa(parameter.R_A,'double')); + +assert(isa(parameter.alpha_m,'double')); +assert(isa(parameter.beta_m,'double')); + +assert(isa(parameter.l_m,'double')); + +assert(isa(dt,'double')); +assert(all(size(dt) == 1)); + +eml.cstructname(parameter,'PropulsionParameters'); + +% Identification of Roxxy2827-34 motor with 10x4.5 propeller +Psi = parameter.Psi; +J_M = parameter.J_M; +R_A = parameter.R_A; +k_m = parameter.k_m; +k_t = parameter.k_t; +alpha_m = parameter.alpha_m; +beta_m = parameter.beta_m; + +% temporarily used Expressions +U = uin(1); +F_m = uin(2); +omega_m = xin(1); + +M_m = k_t*F_m + k_m*omega_m; + +temp = (U*beta_m-Psi*omega_m)./(2*R_A); +I = temp + sqrt(temp.^2 + (U*alpha_m./R_A)); +M_e = Psi*I; % electrical torque motor 1-4 + +% new version +fx = 1/J_M.*(M_e - M_m); + +% old version +% fx = (Psi/R_A*(U-Psi*omega_m) - M_m)/J_M; +% A = -(Psi^2/R_A)/J_M; +% B(1) = Psi/(J_M*R_A); +% B(2) = -1/J_M; + +% system outputs. Use euler solver to predict next time step +% predicted motor speed +xpred = xin + dt*fx; +% electric torque +%y = [M_e I]; + +% system jacobian +% A = 1 + dt*A; +% input jacobian +% B = A*B*dt; + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/parameter_QK_propulsion.m b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/parameter_QK_propulsion.m new file mode 100644 index 0000000..fbb1e87 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/parameter_QK_propulsion.m @@ -0,0 +1,56 @@ +% init-file for quadrotor +rho = 1.225; % [kg/m] Dichte der Luft +g = 9.81; % [m/s] Erdbeschleunigung +w_e = 7.2921151467e-5; % [rad/s] Erddrehrate +c_w = 0.9228*2/3; % [1] Widerstandsbeiwert bei translatorischer Bewegung +c_M = 0.9228*4/5; % [1] Widerstandsbeiwert bei rotatorischer Bewegung +R_p = 0.1269; % [m] Rotorradius +A_xy = 2*0.023*0.2 + .11*0.12; % [m] Flche in x-/y-Achse +A_z = 4*0.023*0.2 + .12^2 + 0*R_p^2*pi*4; % [m] Flche in z-Achse, ohne Rotoren +l_m = 0.275; % [m] Abstand Motor zu Kern +z_m = -0.03; % [m] z-Abstand Propeller-Schwerpunkt +m = 1.477; % [kg] Gesamtmasse des Quadrokopters, ohne Cam, +40g mit Cam +m_M = 0.072; % [kg] Masse Motor + Propeller +m_R = 0.044; % [kg] Masse Rohr +m_C = m - 4*m_M - 2*m_R; % [kg] Masse Kern +I_x = 2*m_M*l_m^2 + 1/12*m_C*(2*.12^2) + 1/12*m_R*(2*l_m)^2; % [kg m] Massentrgheitsmoment um x-Achse +I_y = I_x; % [kg m] Massentrgheitsmoment um y-Achse +I_z = 4*m_M*l_m^2 + 1/12*m_C*(2*.12^2) + 2*1/12*m_R*(2*l_m)^2; % [kg m] Massentrgheitsmoment um z-Achse +inertia = diag([I_x I_y I_z]); % [kg m] Massentrgheitstensor des Quadrokopters +U_max = 14.8; % [V] Maximale Eingangsspannung fr Motor + +% Identification of Roxxy2827-34 motor with 10x4.5 propeller +J_M = 2.5730480633e-5; % [kgm^2] + +% identification from 27.11.2012 with old data, best data +R_A = 0.201084219222241; +Psi = 0.007242179827506; + +% new controller model: u_motor = U_in*(alpha_m/i + beta_m) +alpha_m = 0.104863758313889; +beta_m = 0.549262344777900; + +%% Thrust T = CT*rho*pi*R^4*omega^2 +% J = v_z_Motor*60/(UPM*2*R) = v_z*pi/(R_p*w_M) % Fortschrittsgrad +% CT = CT3*J^3 + CT2*J^2 + CT1*J + CT0 + +% witz normalized rotational speed of motor --> increased matrix condition +CT2s = -1.3077e-2; +CT1s = -2.5224e-4; +CT0s = 1.5393579917e-5; % identification from 15.11.2012 +% torque constant, i.e. Torque = k_t*Thrust +k_t = 0.018228626171312; +k_m = -7.011631909766668e-5; + +%% Drag coefficients + +C_wxy = 1/2*A_xy*rho*c_w; % bezogene Widerstandsgre udot, vdot +C_wz = 1/2*A_z*rho*c_w; % bezogene Widerstandsgre wdot +C_mxy = 1/2*A_z*rho*c_M; % bezogene Widerstandsgre pdot, qdot +C_mz = 1/2*A_xy*rho*c_M; % bezogene Widerstandsgre rdot + +%% Initial conditions + +F0 = m*g; +w0 = sqrt(m*g/4/CT0s); +U0 = R_A/Psi*k_t*m*g/4 + Psi*w0; diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorDrag.m b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorDrag.m new file mode 100644 index 0000000..64a66c4 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorDrag.m @@ -0,0 +1,50 @@ +function y = quadrotorDrag(uin, parameter, dt) +%#eml + +assert(isa(uin,'double')); +assert(all(size(uin) == [6 1])); + +assert(isa(parameter,'struct')); +assert(isa(parameter.C_wxy,'double')); +assert(isa(parameter.C_wz,'double')); + +assert(isa(parameter.C_mxy,'double')); +assert(isa(parameter.C_mz,'double')); + + +assert(isa(dt,'double')); +assert(all(size(dt) == 1)); + +eml.cstructname(parameter,'DragParameters'); + +% initialize vectors +y = zeros(6,1); + +% Input variables +u = uin(1); +v = uin(2); +w = uin(3); +p = uin(4); +q = uin(5); +r = uin(6); + +% Constants +C_wxy = parameter.C_wxy; +C_wz = parameter.C_wz; +C_mxy = parameter.C_mxy; +C_mz = parameter.C_mz; + +% temporarily used vector +absoluteVelocity = sqrt(u^2 + v^2 + w^2); +absoluteAngularVelocity = sqrt(p^2 + q^2 + r^2); + +% system outputs +% calculate drag force +y(1) = C_wxy* absoluteVelocity*u; +y(2) = C_wxy* absoluteVelocity*v; +y(3) = C_wz * absoluteVelocity*w; + +% calculate draq torque +y(4) = C_mxy* absoluteAngularVelocity*p; +y(5) = C_mxy* absoluteAngularVelocity*q; +y(6) = C_mz * absoluteAngularVelocity*r; diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorPropulsion.m b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorPropulsion.m new file mode 100644 index 0000000..f90509d --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorPropulsion.m @@ -0,0 +1,98 @@ +function [y,xpred] = quadrotorPropulsion(xin, uin, parameter, dt) +%#eml + +assert(isa(xin,'double')); +assert(all(size(xin) == [4 1])); + +assert(isa(uin,'double')); +assert(all(size(uin) == [10 1])); + +assert(isa(parameter,'struct')); +assert(isa(parameter.k_m,'double')); +assert(isa(parameter.k_t,'double')); + +assert(isa(parameter.CT2s,'double')); +assert(isa(parameter.CT1s,'double')); +assert(isa(parameter.CT0s,'double')); + +assert(isa(parameter.Psi,'double')); +assert(isa(parameter.J_M,'double')); +assert(isa(parameter.R_A,'double')); + +assert(isa(parameter.alpha_m,'double')); +assert(isa(parameter.beta_m,'double')); + +assert(isa(parameter.l_m,'double')); + +assert(isa(dt,'double')); +assert(all(size(dt) == 1)); + +eml.cstructname(parameter,'PropulsionParameters'); + +% initialize vectors +xpred = xin; % motorspeed +v_1 = zeros(4,1); +y = zeros(14,1); +F_m = zeros(4,1); +U = zeros(4,1); +M_e = zeros(4,1); +I = zeros(4,1); +F = zeros(3,1); + +% Input variables +u = uin(1); +v = uin(2); +w = uin(3); +p = uin(4); +q = uin(5); +r = uin(6); +U(1) = uin(7); +U(2) = uin(8); +U(3) = uin(9); +U(4) = uin(10); + +% Constants +CT0s = parameter.CT0s; +CT1s = parameter.CT1s; +CT2s = parameter.CT2s; +k_t = parameter.k_t; +l_m = parameter.l_m; +Psi = parameter.Psi; + +v_1(1) = - w + l_m*q; +v_1(2) = - w - l_m*p; +v_1(3) = - w - l_m*q; +v_1(4) = - w + l_m*p; + +% calculate thrust for all 4 rotors +for i = 1:4 + % if the flow speed at infinity is negative + if v_1(i) < 0 + F_m(i) = CT2s*v_1(i).^2 + CT1s*v_1(i).*xin(i) + CT0s*xin(i).^2; + % if the flow speed at infinity is positive + else + F_m(i) = -CT2s*v_1(i).^2 + CT1s*v_1(i).*xin(i) + CT0s*xin(i).^2; + end + % sum up all rotor forces + F(3) = F(3) + F_m(i) ; + + [M_e(i),I(i),xpred(i)] = motorspeed(xin(i), [U(i) F_m(i)], parameter, dt); +end + +% System output, i.e. force and torque of quadrotor +y(1) = F(1); +y(2) = F(2); +y(3) = F(3); + +% torque for rotating quadrocopter around x-axis is the mechanical torque +y(4) = (F_m(4)-F_m(2))*l_m; +% torque for rotating quadrocopter around y-axis is the mechanical torque +y(5) = (F_m(1)-F_m(3))*l_m; +% torque for rotating quadrocopter around z-axis is the electrical torque +y(6) = (-M_e(1)-M_e(3)+M_e(2)+M_e(4)); + +% motor speeds (rad/s) +y(7:10) = xpred(1:4); + +% motor current (A) +y(11:14) = I(1:4); % M_e(1:4) / Psi; diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/package.xml new file mode 100644 index 0000000..e38afd3 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/package.xml @@ -0,0 +1,34 @@ + + hector_quadrotor_model + 0.3.5 + hector_quadrotor_model provides libraries that model several aspects of quadrotor dynamics. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_model + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + Alexander Sendobry + + + catkin + + + geometry_msgs + hector_uav_msgs + eigen + roscpp + cmake_modules + boost + + + geometry_msgs + hector_uav_msgs + roscpp + boost + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/quadrotor_aerodynamics.yaml b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/quadrotor_aerodynamics.yaml new file mode 100644 index 0000000..3c4e3cf --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/quadrotor_aerodynamics.yaml @@ -0,0 +1,5 @@ +quadrotor_aerodynamics: + C_wxy: 0.12 + C_wz: 0.1 + C_mxy: 0.074156208000000 + C_mz: 0.050643264000000 \ No newline at end of file diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1045.yaml b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1045.yaml new file mode 100644 index 0000000..7799bcf --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1045.yaml @@ -0,0 +1,15 @@ +quadrotor_propulsion: + Psi: 0.007242179827506 + J_M: 2.573048063300000e-5 + R_A: 0.201084219222241 + k_t: 0.015336864714397 + k_m: -7.011631909766668e-5 + alpha_m: 0.104863758313889 + beta_m: 0.549262344777900 + + #CT2s: -1.3077e-2 + CT2s: 0.0 + CT1s: -2.5224e-4 + CT0s: 1.538190483976698e-5 + + l_m: 0.275 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1245.yaml b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1245.yaml new file mode 100644 index 0000000..fd6fcc0 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1245.yaml @@ -0,0 +1,12 @@ +quadrotor_propulsion: + Psi: 0.007242179827506 + J_M: 4.142069415e-05 + R_A: 0.201084219222241 + k_t: 0.01732804081 + + #CT2s: -2.3491553043010e-2 + CT2s: 0.0 + CT1s: -4.531245193522030e-04 + CT0s: 2.744543453e-05 + + l_m: 0.275 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/matlab_helpers.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/matlab_helpers.h new file mode 100644 index 0000000..bcd0faa --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/matlab_helpers.h @@ -0,0 +1,97 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_MODEL_MATLAB_HELPERS_H +#define HECTOR_QUADROTOR_MODEL_MATLAB_HELPERS_H + +#include +#include + +typedef double real_T; +typedef uint32_t int32_T; +typedef bool boolean_T; + +static const real_T rtInf = std::numeric_limits::infinity(); +static inline boolean_T rtIsInf(real_T value) +{ + return std::isinf(value); +} + +static const real_T rtNaN = std::numeric_limits::quiet_NaN(); +static inline boolean_T rtIsNaN(real_T value) +{ + return std::isnan(value); +} + +static real_T rt_powd_snf(real_T u0, real_T u1) +{ + real_T y; + real_T d0; + real_T d1; + if (rtIsNaN(u0) || rtIsNaN(u1)) { + y = rtNaN; + } else { + d0 = fabs(u0); + d1 = fabs(u1); + if (rtIsInf(u1)) { + if (d0 == 1.0) { + y = rtNaN; + } else if (d0 > 1.0) { + if (u1 > 0.0) { + y = rtInf; + } else { + y = 0.0; + } + } else if (u1 > 0.0) { + y = 0.0; + } else { + y = rtInf; + } + } else if (d1 == 0.0) { + y = 1.0; + } else if (d1 == 1.0) { + if (u1 > 0.0) { + y = u0; + } else { + y = 1.0 / u0; + } + } else if (u1 == 2.0) { + y = u0 * u0; + } else if ((u1 == 0.5) && (u0 >= 0.0)) { + y = sqrt(u0); + } else if ((u0 < 0.0) && (u1 > floor(u1))) { + y = rtNaN; + } else { + y = pow(u0, u1); + } + } + + return y; +} + +#endif // HECTOR_QUADROTOR_MODEL_MATLAB_HELPERS_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_aerodynamics.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_aerodynamics.cpp new file mode 100644 index 0000000..2bb9baf --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_aerodynamics.cpp @@ -0,0 +1,254 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include +#include + +//extern "C" { +// #include "quadrotorDrag/quadrotorDrag.h" +// #include "quadrotorDrag/quadrotorDrag_initialize.h" +//} + +#include +#include + +#include "matlab_helpers.h" + +namespace hector_quadrotor_model { + +struct DragParameters +{ + real_T C_wxy; + real_T C_wz; + real_T C_mxy; + real_T C_mz; + + DragParameters() + : C_wxy(0.0) + , C_wz(0.0) + , C_mxy(0.0) + , C_mz(0.0) + {} +}; + +// extern void quadrotorDrag(const real_T uin[6], const DragParameters parameter, real_T dt, real_T y[6]); +struct QuadrotorAerodynamics::DragModel { + DragParameters parameters_; + boost::array u; + boost::array y; +}; + +QuadrotorAerodynamics::QuadrotorAerodynamics() +{ + // initialize drag model + // quadrotorDrag_initialize(); + drag_model_ = new DragModel; +} + +QuadrotorAerodynamics::~QuadrotorAerodynamics() +{ + delete drag_model_; +} + +/* + * quadrotorDrag.c + * + * Code generation for function 'quadrotorDrag' + * + * C source code generated on: Sun Nov 3 13:34:38 2013 + * + */ + +/* Include files */ +//#include "rt_nonfinite.h" +//#include "quadrotorDrag.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Definitions */ +void quadrotorDrag(const real_T uin[6], const DragParameters parameter, real_T + dt, real_T y[6]) +{ + int32_T i; + real_T absoluteVelocity; + real_T absoluteAngularVelocity; + + /* initialize vectors */ + for (i = 0; i < 6; i++) { + y[i] = 0.0; + } + + /* Input variables */ + /* Constants */ + /* temporarily used vector */ + absoluteVelocity = sqrt((rt_powd_snf(uin[0], 2.0) + rt_powd_snf(uin[1], 2.0)) + + rt_powd_snf(uin[2], 2.0)); + absoluteAngularVelocity = sqrt((rt_powd_snf(uin[3], 2.0) + rt_powd_snf(uin[4], + 2.0)) + rt_powd_snf(uin[5], 2.0)); + + /* system outputs */ + /* calculate drag force */ + y[0] = parameter.C_wxy * absoluteVelocity * uin[0]; + y[1] = parameter.C_wxy * absoluteVelocity * uin[1]; + y[2] = parameter.C_wz * absoluteVelocity * uin[2]; + + /* calculate draq torque */ + y[3] = parameter.C_mxy * absoluteAngularVelocity * uin[3]; + y[4] = parameter.C_mxy * absoluteAngularVelocity * uin[4]; + y[5] = parameter.C_mz * absoluteAngularVelocity * uin[5]; +} + +/* End of code generation (quadrotorDrag.c) */ + +inline void QuadrotorAerodynamics::f(const double uin[10], double dt, double y[14]) const +{ + quadrotorDrag(uin, drag_model_->parameters_, dt, y); +} + +bool QuadrotorAerodynamics::configure(const ros::NodeHandle ¶m) +{ + // get model parameters + if (!param.getParam("C_wxy", drag_model_->parameters_.C_wxy)) return false; + if (!param.getParam("C_wz", drag_model_->parameters_.C_wz)) return false; + if (!param.getParam("C_mxy", drag_model_->parameters_.C_mxy)) return false; + if (!param.getParam("C_mz", drag_model_->parameters_.C_mz)) return false; + +#ifndef NDEBUG + std::cout << "Loaded the following quadrotor drag model parameters from namespace " << param.getNamespace() << ":" << std::endl; + std::cout << "C_wxy = " << drag_model_->parameters_.C_wxy << std::endl; + std::cout << "C_wz = " << drag_model_->parameters_.C_wz << std::endl; + std::cout << "C_mxy = " << drag_model_->parameters_.C_mxy << std::endl; + std::cout << "C_mz = " << drag_model_->parameters_.C_mz << std::endl; +#endif + + reset(); + return true; +} + +void QuadrotorAerodynamics::reset() +{ + boost::mutex::scoped_lock lock(mutex_); + drag_model_->u.assign(0.0); + drag_model_->y.assign(0.0); + + twist_ = geometry_msgs::Twist(); + wind_ = geometry_msgs::Vector3(); + wrench_ = geometry_msgs::Wrench(); +} + +void QuadrotorAerodynamics::setOrientation(const geometry_msgs::Quaternion& orientation) +{ + boost::mutex::scoped_lock lock(mutex_); + orientation_ = orientation; +} + +void QuadrotorAerodynamics::setTwist(const geometry_msgs::Twist& twist) +{ + boost::mutex::scoped_lock lock(mutex_); + twist_ = twist; +} + +void QuadrotorAerodynamics::setBodyTwist(const geometry_msgs::Twist& body_twist) +{ + boost::mutex::scoped_lock lock(mutex_); + Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z); + Eigen::Matrix inverse_rotation_matrix(orientation.inverse().toRotationMatrix()); + + Eigen::Vector3d body_linear(body_twist.linear.x, body_twist.linear.y, body_twist.linear.z); + Eigen::Vector3d world_linear(inverse_rotation_matrix * body_linear); + twist_.linear.x = world_linear.x(); + twist_.linear.y = world_linear.y(); + twist_.linear.z = world_linear.z(); + + Eigen::Vector3d body_angular(body_twist.angular.x, body_twist.angular.y, body_twist.angular.z); + Eigen::Vector3d world_angular(inverse_rotation_matrix * body_angular); + twist_.angular.x = world_angular.x(); + twist_.angular.y = world_angular.y(); + twist_.angular.z = world_angular.z(); +} + +void QuadrotorAerodynamics::setWind(const geometry_msgs::Vector3& wind) +{ + boost::mutex::scoped_lock lock(mutex_); + wind_ = wind; +} + +void QuadrotorAerodynamics::update(double dt) +{ + if (dt <= 0.0) return; + boost::mutex::scoped_lock lock(mutex_); + + // fill input vector u for drag model + drag_model_->u[0] = (twist_.linear.x - wind_.x); + drag_model_->u[1] = -(twist_.linear.y - wind_.y); + drag_model_->u[2] = -(twist_.linear.z - wind_.z); + drag_model_->u[3] = twist_.angular.x; + drag_model_->u[4] = -twist_.angular.y; + drag_model_->u[5] = -twist_.angular.z; + + // We limit the input velocities to +-100. Required for numeric stability in case of collisions, + // where velocities returned by Gazebo can be very high. + limit(drag_model_->u, -100.0, 100.0); + + // convert input to body coordinates + Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z); + Eigen::Matrix rotation_matrix(orientation.toRotationMatrix()); + Eigen::Map linear(&(drag_model_->u[0])); + Eigen::Map angular(&(drag_model_->u[3])); + linear = rotation_matrix * linear; + angular = rotation_matrix * angular; + + ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.twist: " << PrintVector(drag_model_->u.begin(), drag_model_->u.begin() + 6)); + checknan(drag_model_->u, "drag model input"); + + // update drag model + f(drag_model_->u.data(), dt, drag_model_->y.data()); + + ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.force: " << PrintVector(drag_model_->y.begin() + 0, drag_model_->y.begin() + 3)); + ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.torque: " << PrintVector(drag_model_->y.begin() + 3, drag_model_->y.begin() + 6)); + checknan(drag_model_->y, "drag model output"); + + // drag_model_ gives us inverted vectors! + wrench_.force.x = -( drag_model_->y[0]); + wrench_.force.y = -(-drag_model_->y[1]); + wrench_.force.z = -(-drag_model_->y[2]); + wrench_.torque.x = -( drag_model_->y[3]); + wrench_.torque.y = -(-drag_model_->y[4]); + wrench_.torque.z = -(-drag_model_->y[5]); +} + +} // namespace hector_quadrotor_model diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_propulsion.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_propulsion.cpp new file mode 100644 index 0000000..ea308a0 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_model/src/quadrotor_propulsion.cpp @@ -0,0 +1,488 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +#include +#include + +#include + +#include "matlab_helpers.h" + +//extern "C" { +// #include "quadrotorPropulsion/quadrotorPropulsion.h" +// #include "quadrotorPropulsion/quadrotorPropulsion_initialize.h" +//} + +namespace hector_quadrotor_model { + +struct PropulsionParameters +{ + real_T k_m; + real_T k_t; + real_T CT2s; + real_T CT1s; + real_T CT0s; + real_T Psi; + real_T J_M; + real_T R_A; + real_T alpha_m; + real_T beta_m; + real_T l_m; + + PropulsionParameters() + : k_m(0.0) + , k_t(0.0) + , CT2s(0.0) + , CT1s(0.0) + , CT0s(0.0) + , Psi(0.0) + , J_M(std::numeric_limits::infinity()) + , R_A(std::numeric_limits::infinity()) + , alpha_m(0.0) + , beta_m(0.0) + , l_m(0.0) + {} +}; + +struct QuadrotorPropulsion::PropulsionModel { + PropulsionParameters parameters_; + boost::array x; + boost::array x_pred; + boost::array u; + boost::array y; +}; + +QuadrotorPropulsion::QuadrotorPropulsion() +{ + // initialize propulsion model + // quadrotorPropulsion_initialize(); + propulsion_model_ = new PropulsionModel; +} + +QuadrotorPropulsion::~QuadrotorPropulsion() +{ + delete propulsion_model_; +} + +/* + * quadrotorPropulsion.c + * + * Code generation for function 'quadrotorPropulsion' + * + * C source code generated on: Sun Nov 3 13:34:35 2013 + * + */ + +/* Include files */ +//#include "rt_nonfinite.h" +//#include "motorspeed.h" +//#include "quadrotorPropulsion.h" +//#include "quadrotorPropulsion_rtwutil.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void quadrotorPropulsion(const real_T xin[4], const real_T uin[10], const + PropulsionParameters parameter, real_T dt, real_T y[14], real_T xpred[4]) +{ + real_T v_1[4]; + int32_T i; + real_T F_m[4]; + real_T U[4]; + real_T M_e[4]; + real_T I[4]; + real_T F[3]; + real_T b_F_m; + real_T temp; + real_T d0; + + /* initialize vectors */ + for (i = 0; i < 4; i++) { + xpred[i] = xin[i]; + + /* motorspeed */ + v_1[i] = 0.0; + } + + memset(&y[0], 0, 14U * sizeof(real_T)); + for (i = 0; i < 4; i++) { + F_m[i] = 0.0; + U[i] = 0.0; + M_e[i] = 0.0; + I[i] = 0.0; + } + + for (i = 0; i < 3; i++) { + F[i] = 0.0; + } + + /* Input variables */ + U[0] = uin[6]; + U[1] = uin[7]; + U[2] = uin[8]; + U[3] = uin[9]; + + /* Constants */ + v_1[0] = -uin[2] + parameter.l_m * uin[4]; + v_1[1] = -uin[2] - parameter.l_m * uin[3]; + v_1[2] = -uin[2] - parameter.l_m * uin[4]; + v_1[3] = -uin[2] + parameter.l_m * uin[3]; + + /* calculate thrust for all 4 rotors */ + for (i = 0; i < 4; i++) { + /* if the flow speed at infinity is negative */ + if (v_1[i] < 0.0) { + b_F_m = (parameter.CT2s * rt_powd_snf(v_1[i], 2.0) + parameter.CT1s * + v_1[i] * xin[i]) + parameter.CT0s * rt_powd_snf(xin[i], 2.0); + + /* if the flow speed at infinity is positive */ + } else { + b_F_m = (-parameter.CT2s * rt_powd_snf(v_1[i], 2.0) + parameter.CT1s * + v_1[i] * xin[i]) + parameter.CT0s * rt_powd_snf(xin[i], 2.0); + } + + /* sum up all rotor forces */ + /* Identification of Roxxy2827-34 motor with 10x4.5 propeller */ + /* temporarily used Expressions */ + temp = (U[i] * parameter.beta_m - parameter.Psi * xin[i]) / (2.0 * + parameter.R_A); + temp += sqrt(rt_powd_snf(temp, 2.0) + U[i] * parameter.alpha_m / + parameter.R_A); + d0 = parameter.Psi * temp; + + /* electrical torque motor 1-4 */ + /* new version */ + /* old version */ + /* fx = (Psi/R_A*(U-Psi*omega_m) - M_m)/J_M; */ + /* A = -(Psi^2/R_A)/J_M; */ + /* B(1) = Psi/(J_M*R_A); */ + /* B(2) = -1/J_M; */ + /* system outputs. Use euler solver to predict next time step */ + /* predicted motor speed */ + /* electric torque */ + /* y = [M_e I]; */ + /* system jacobian */ + /* A = 1 + dt*A; */ + /* input jacobian */ + /* B = A*B*dt; */ + M_e[i] = d0; + I[i] = temp; + xpred[i] = xin[i] + dt * (1.0 / parameter.J_M * (d0 - (parameter.k_t * b_F_m + + parameter.k_m * xin[i]))); + F_m[i] = b_F_m; + F[2] += b_F_m; + } + + /* System output, i.e. force and torque of quadrotor */ + y[0] = 0.0; + y[1] = 0.0; + y[2] = F[2]; + + /* torque for rotating quadrocopter around x-axis is the mechanical torque */ + y[3] = (F_m[3] - F_m[1]) * parameter.l_m; + + /* torque for rotating quadrocopter around y-axis is the mechanical torque */ + y[4] = (F_m[0] - F_m[2]) * parameter.l_m; + + /* torque for rotating quadrocopter around z-axis is the electrical torque */ + y[5] = ((-M_e[0] - M_e[2]) + M_e[1]) + M_e[3]; + + /* motor speeds (rad/s) */ + for (i = 0; i < 4; i++) { + y[i + 6] = xpred[i]; + } + + /* motor current (A) */ + for (i = 0; i < 4; i++) { + y[i + 10] = I[i]; + } + + /* M_e(1:4) / Psi; */ +} + +/* End of code generation (quadrotorPropulsion.c) */ + +inline void QuadrotorPropulsion::f(const double xin[4], const double uin[10], double dt, double y[14], double xpred[4]) const +{ + quadrotorPropulsion(xin, uin, propulsion_model_->parameters_, dt, y, xpred); +} + +bool QuadrotorPropulsion::configure(const ros::NodeHandle ¶m) +{ + // get model parameters + if (!param.getParam("k_m", propulsion_model_->parameters_.k_m)) return false; + if (!param.getParam("k_t", propulsion_model_->parameters_.k_t)) return false; + if (!param.getParam("CT0s", propulsion_model_->parameters_.CT0s)) return false; + if (!param.getParam("CT1s", propulsion_model_->parameters_.CT1s)) return false; + if (!param.getParam("CT2s", propulsion_model_->parameters_.CT2s)) return false; + if (!param.getParam("J_M", propulsion_model_->parameters_.J_M)) return false; + if (!param.getParam("l_m", propulsion_model_->parameters_.l_m)) return false; + if (!param.getParam("Psi", propulsion_model_->parameters_.Psi)) return false; + if (!param.getParam("R_A", propulsion_model_->parameters_.R_A)) return false; + if (!param.getParam("alpha_m", propulsion_model_->parameters_.alpha_m)) return false; + if (!param.getParam("beta_m", propulsion_model_->parameters_.beta_m)) return false; + +#ifndef NDEBUG + std::cout << "Loaded the following quadrotor propulsion model parameters from namespace " << param.getNamespace() << ":" << std::endl; + std::cout << "k_m = " << propulsion_model_->parameters_.k_m << std::endl; + std::cout << "k_t = " << propulsion_model_->parameters_.k_t << std::endl; + std::cout << "CT2s = " << propulsion_model_->parameters_.CT2s << std::endl; + std::cout << "CT1s = " << propulsion_model_->parameters_.CT1s << std::endl; + std::cout << "CT0s = " << propulsion_model_->parameters_.CT0s << std::endl; + std::cout << "Psi = " << propulsion_model_->parameters_.Psi << std::endl; + std::cout << "J_M = " << propulsion_model_->parameters_.J_M << std::endl; + std::cout << "R_A = " << propulsion_model_->parameters_.R_A << std::endl; + std::cout << "l_m = " << propulsion_model_->parameters_.l_m << std::endl; + std::cout << "alpha_m = " << propulsion_model_->parameters_.alpha_m << std::endl; + std::cout << "beta_m = " << propulsion_model_->parameters_.beta_m << std::endl; +#endif + + initial_voltage_ = 14.8; + param.getParam("supply_voltage", initial_voltage_); + reset(); + + return true; +} + +void QuadrotorPropulsion::reset() +{ + boost::mutex::scoped_lock lock(mutex_); + + propulsion_model_->x.assign(0.0); + propulsion_model_->x_pred.assign(0.0); + propulsion_model_->u.assign(0.0); + propulsion_model_->y.assign(0.0); + + wrench_ = geometry_msgs::Wrench(); + + motor_status_ = hector_uav_msgs::MotorStatus(); + motor_status_.voltage.resize(4); + motor_status_.frequency.resize(4); + motor_status_.current.resize(4); + + supply_ = hector_uav_msgs::Supply(); + supply_.voltage.resize(1); + supply_.voltage[0] = initial_voltage_; + + last_command_time_ = ros::Time(); + + command_queue_ = std::queue(); // .clear(); +} + +void QuadrotorPropulsion::setVoltage(const hector_uav_msgs::MotorPWM& voltage) +{ + boost::mutex::scoped_lock lock(mutex_); + last_command_time_ = voltage.header.stamp; + + if (motor_status_.on && voltage.pwm.size() >= 4) { + propulsion_model_->u[6] = voltage.pwm[0] * supply_.voltage[0] / 255.0; + propulsion_model_->u[7] = voltage.pwm[1] * supply_.voltage[0] / 255.0; + propulsion_model_->u[8] = voltage.pwm[2] * supply_.voltage[0] / 255.0; + propulsion_model_->u[9] = voltage.pwm[3] * supply_.voltage[0] / 255.0; + } else { + propulsion_model_->u[6] = 0.0; + propulsion_model_->u[7] = 0.0; + propulsion_model_->u[8] = 0.0; + propulsion_model_->u[9] = 0.0; + } +} + +void QuadrotorPropulsion::setTwist(const geometry_msgs::Twist &twist) +{ + boost::mutex::scoped_lock lock(mutex_); + propulsion_model_->u[0] = twist.linear.x; + propulsion_model_->u[1] = -twist.linear.y; + propulsion_model_->u[2] = -twist.linear.z; + propulsion_model_->u[3] = twist.angular.x; + propulsion_model_->u[4] = -twist.angular.y; + propulsion_model_->u[5] = -twist.angular.z; + + // We limit the input velocities to +-100. Required for numeric stability in case of collisions, + // where velocities returned by Gazebo can be very high. + limit(boost::iterator_range::iterator>(&(propulsion_model_->u[0]), &(propulsion_model_->u[6])), -100.0, 100.0); +} + +void QuadrotorPropulsion::addCommandToQueue(const hector_uav_msgs::MotorCommandConstPtr& command) +{ + hector_uav_msgs::MotorPWMPtr pwm(new hector_uav_msgs::MotorPWM); + pwm->header = command->header; + pwm->pwm.resize(command->voltage.size()); + for(std::size_t i = 0; i < command->voltage.size(); ++i) { + int temp = round(command->voltage[i] / supply_.voltage[0] * 255.0); + if (temp < 0) + pwm->pwm[i] = 0; + else if (temp > 255) + pwm->pwm[i] = 255; + else + pwm->pwm[i] = temp; + } + addPWMToQueue(pwm); +} + +void QuadrotorPropulsion::addPWMToQueue(const hector_uav_msgs::MotorPWMConstPtr& pwm) +{ + boost::mutex::scoped_lock lock(command_queue_mutex_); + + if (!motor_status_.on) { + ROS_WARN_NAMED("quadrotor_propulsion", "Received new motor command. Enabled motors."); + engage(); + } + + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Received motor command valid at " << pwm->header.stamp); + command_queue_.push(pwm); + command_condition_.notify_all(); +} + +bool QuadrotorPropulsion::processQueue(const ros::Time ×tamp, const ros::Duration &tolerance, const ros::Duration &delay, const ros::WallDuration &wait, ros::CallbackQueue *callback_queue) { + boost::mutex::scoped_lock lock(command_queue_mutex_); + bool new_command = false; + + ros::Time min(timestamp), max(timestamp); + try { + min = timestamp - delay - tolerance /* - ros::Duration(dt) */; + } catch (std::runtime_error &e) {} + + try { + max = timestamp - delay + tolerance; + } catch (std::runtime_error &e) {} + + do { + + while(!command_queue_.empty()) { + hector_uav_msgs::MotorPWMConstPtr new_motor_voltage = command_queue_.front(); + ros::Time new_time = new_motor_voltage->header.stamp; + + if (new_time.isZero() || (new_time >= min && new_time <= max)) { + setVoltage(*new_motor_voltage); + command_queue_.pop(); + new_command = true; + + // new motor command is too old: + } else if (new_time < min) { + ROS_DEBUG_NAMED("quadrotor_propulsion", "Command received was %fs too old. Discarding.", (new_time - timestamp).toSec()); + command_queue_.pop(); + + // new motor command is too new: + } else { + break; + } + } + + if (command_queue_.empty() && !new_command) { + if (!motor_status_.on || wait.isZero()) break; + + ROS_DEBUG_NAMED("quadrotor_propulsion", "Waiting for command at simulation step t = %fs... last update was %fs ago", timestamp.toSec(), (timestamp - last_command_time_).toSec()); + if (!callback_queue) { + if (command_condition_.timed_wait(lock, wait.toBoost())) continue; + } else { + lock.unlock(); + callback_queue->callAvailable(wait); + lock.lock(); + if (!command_queue_.empty()) continue; + } + + ROS_ERROR_NAMED("quadrotor_propulsion", "Command timed out. Disabled motors."); + shutdown(); + } + + } while(false); + + if (new_command) { + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Using motor command valid at t = " << last_command_time_.toSec() << "s for simulation step at t = " << timestamp.toSec() << "s (dt = " << (timestamp - last_command_time_).toSec() << "s)"); + } + + return new_command; +} + +void QuadrotorPropulsion::update(double dt) +{ + if (dt <= 0.0) return; + + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.twist: " << PrintVector(propulsion_model_->u.begin(), propulsion_model_->u.begin() + 6)); + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.voltage: " << PrintVector(propulsion_model_->u.begin() + 6, propulsion_model_->u.begin() + 10)); + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_prior: " << PrintVector(propulsion_model_->x.begin(), propulsion_model_->x.end())); + + checknan(propulsion_model_->u, "propulsion model input"); + checknan(propulsion_model_->x, "propulsion model state"); + + // update propulsion model + f(propulsion_model_->x.data(), propulsion_model_->u.data(), dt, propulsion_model_->y.data(), propulsion_model_->x_pred.data()); + propulsion_model_->x = propulsion_model_->x_pred; + + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.x_post: " << PrintVector(propulsion_model_->x.begin(), propulsion_model_->x.end())); + ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "propulsion.force: " << PrintVector(propulsion_model_->y.begin() + 0, propulsion_model_->y.begin() + 3) << " " << + "propulsion.torque: " << PrintVector(propulsion_model_->y.begin() + 3, propulsion_model_->y.begin() + 6)); + + checknan(propulsion_model_->y, "propulsion model output"); + + wrench_.force.x = propulsion_model_->y[0]; + wrench_.force.y = -propulsion_model_->y[1]; + wrench_.force.z = propulsion_model_->y[2]; + wrench_.torque.x = propulsion_model_->y[3]; + wrench_.torque.y = -propulsion_model_->y[4]; + wrench_.torque.z = -propulsion_model_->y[5]; + + motor_status_.voltage[0] = propulsion_model_->u[6]; + motor_status_.voltage[1] = propulsion_model_->u[7]; + motor_status_.voltage[2] = propulsion_model_->u[8]; + motor_status_.voltage[3] = propulsion_model_->u[9]; + + motor_status_.frequency[0] = propulsion_model_->y[6]; + motor_status_.frequency[1] = propulsion_model_->y[7]; + motor_status_.frequency[2] = propulsion_model_->y[8]; + motor_status_.frequency[3] = propulsion_model_->y[9]; + motor_status_.running = motor_status_.frequency[0] > 1.0 && motor_status_.frequency[1] > 1.0 && motor_status_.frequency[2] > 1.0 && motor_status_.frequency[3] > 1.0; + + motor_status_.current[0] = propulsion_model_->y[10]; + motor_status_.current[1] = propulsion_model_->y[11]; + motor_status_.current[2] = propulsion_model_->y[12]; + motor_status_.current[3] = propulsion_model_->y[13]; +} + +void QuadrotorPropulsion::engage() +{ + motor_status_.on = true; +} + +void QuadrotorPropulsion::shutdown() +{ + motor_status_.on = false; +} + +} // namespace hector_quadrotor_model diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CHANGELOG.rst new file mode 100644 index 0000000..a7acba8 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CHANGELOG.rst @@ -0,0 +1,37 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_pose_estimation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ +* added missing install rule for hector_quadrotor_pose_estimation_nodelets.xml + Many thanks to Bernd Kast for pointing me to this issue. +* update raw baro height as position.z component in sensor pose + See https://github.com/tu-darmstadt-ros-pkg/hector_localization/commit/bd334f0e30c42fb5833fa8ffa249dfd737d43ddc. +* updated package for the latest version of hector_pose_estimation + * Geographic reference latitude and longitude set to 49.860246N 8.687077E (Lichtwiese). + * Reenabled auto_elevation, auto_reference and auto_heading parameters for hector_pose_estimation. + hector_pose_estimation will publish the world->nav transform depending on its reference pose. +* added parameter file for hector_quadrotor_pose_estimation for a simulated quadrotor + The parameter file disables the auto_elevation, auto_reference, auto_heading modes of hector_pose_estimation and sets the corresponding values + manually with what is simulated in the gazebo sensor plugins. This simplifies the comparison of estimated poses with ground truth information. +* shutdown the height subscriber (in favor of topic alitimeter) to not have two height updates +* Contributors: Johannes Meyer + +0.3.3 (2014-09-01) +------------------ + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ + +0.3.0 (2013-09-11) +------------------ +* hector_quadrotor_pose_estimation: added cmake target dependency on hector_uav_msgs_generate_messages_cpp +* hector_quadrotor: added package hector_quadrotor_pose_estimation +* Contributors: Johannes Meyer diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CMakeLists.txt new file mode 100644 index 0000000..a11c4f6 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/CMakeLists.txt @@ -0,0 +1,112 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_pose_estimation) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS hector_pose_estimation hector_uav_msgs) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# hector_uav_msgs +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES quadrotor_pose_estimation_nodelet quadrotor_pose_estimation_node + CATKIN_DEPENDS hector_pose_estimation hector_uav_msgs + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include) +include_directories(${catkin_INCLUDE_DIRS}) + +add_library(hector_quadrotor_pose_estimation_node + src/pose_estimation_node.cpp +) +target_link_libraries(hector_quadrotor_pose_estimation_node ${catkin_LIBRARIES}) +add_dependencies(hector_quadrotor_pose_estimation_node hector_uav_msgs_generate_messages_cpp) + +add_library(hector_quadrotor_pose_estimation_nodelet + src/pose_estimation_nodelet.cpp +) +target_link_libraries(hector_quadrotor_pose_estimation_nodelet hector_quadrotor_pose_estimation_node ${catkin_LIBRARIES}) + +add_executable(hector_quadrotor_pose_estimation + src/main.cpp +) +target_link_libraries(hector_quadrotor_pose_estimation hector_quadrotor_pose_estimation_node ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executables and/or libraries for installation +install(TARGETS hector_quadrotor_pose_estimation_nodelet hector_quadrotor_pose_estimation_node hector_quadrotor_pose_estimation + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + hector_quadrotor_pose_estimation_nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY params + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/hector_quadrotor_pose_estimation_nodelets.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/hector_quadrotor_pose_estimation_nodelets.xml new file mode 100644 index 0000000..f2b2132 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/hector_quadrotor_pose_estimation_nodelets.xml @@ -0,0 +1,8 @@ + + + + This nodelet implements a pose estimation filter which uses hector_uav_msgs/Altimeter messages as input for barometric height. + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/include/hector_quadrotor_pose_estimation/pose_estimation_node.h b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/include/hector_quadrotor_pose_estimation/pose_estimation_node.h new file mode 100644 index 0000000..b72366c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/include/hector_quadrotor_pose_estimation/pose_estimation_node.h @@ -0,0 +1,55 @@ +//================================================================================================= +// Copyright (c) 2011, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_QUADROTOR_POSE_ESTIMATION_NODE_H +#define HECTOR_QUADROTOR_POSE_ESTIMATION_NODE_H + +#include +#include + +namespace hector_quadrotor_pose_estimation { + +using namespace hector_pose_estimation; + +class QuadrotorPoseEstimationNode : public PoseEstimationNode { +public: + QuadrotorPoseEstimationNode(const SystemPtr& system = SystemPtr(), const StatePtr& state = StatePtr()); + virtual ~QuadrotorPoseEstimationNode(); + + virtual bool init(); + +protected: + void baroCallback(const hector_uav_msgs::AltimeterConstPtr& altimeter); + +private: + ros::Subscriber baro_subscriber_; +}; + +} // namespace hector_quadrotor_pose_estimation + +#endif // HECTOR_QUADROTOR_POSE_ESTIMATION_NODE_H diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/package.xml new file mode 100644 index 0000000..76f40fd --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/package.xml @@ -0,0 +1,34 @@ + + + hector_quadrotor_pose_estimation + 0.3.5 + + hector_quadrotor_pose_estimation provides a hector_pose_estimation node and nodelet specialized for hector_quadrotor. + + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_pose_estimation + + Johannes Meyer + + + catkin + + + hector_pose_estimation + hector_uav_msgs + + + hector_pose_estimation + hector_uav_msgs + nodelet + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/params/simulation.yaml b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/params/simulation.yaml new file mode 100644 index 0000000..044614b --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/params/simulation.yaml @@ -0,0 +1,4 @@ +reference_altitude: 0.0 +reference_heading: 0.0 +reference_latitude: 49.860246 +reference_longitude: 8.687077 diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/main.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/main.cpp new file mode 100644 index 0000000..dde98d8 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/main.cpp @@ -0,0 +1,40 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include + +int main(int argc, char **argv) { + ros::init(argc, argv, "pose_estimation"); + hector_quadrotor_pose_estimation::QuadrotorPoseEstimationNode node; + if (!node.init()) return 1; + + ros::spin(); + + node.cleanup(); + return 0; +} diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_node.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_node.cpp new file mode 100644 index 0000000..b249331 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_node.cpp @@ -0,0 +1,60 @@ +//================================================================================================= +// Copyright (c) 2011, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +namespace hector_quadrotor_pose_estimation { + +QuadrotorPoseEstimationNode::QuadrotorPoseEstimationNode(const SystemPtr& system, const StatePtr& state) + : PoseEstimationNode(system, state) +{ + pose_estimation_->addMeasurement(new Baro("baro")); +} + +QuadrotorPoseEstimationNode::~QuadrotorPoseEstimationNode() +{ +} + +bool QuadrotorPoseEstimationNode::init() { + if (!PoseEstimationNode::init()) return false; + baro_subscriber_ = getNodeHandle().subscribe("altimeter", 10, &QuadrotorPoseEstimationNode::baroCallback, this); + height_subscriber_.shutdown(); + return true; +} + +void QuadrotorPoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& altimeter) { + pose_estimation_->getMeasurement("baro")->add(Baro::Update(altimeter->pressure, altimeter->qnh)); + + if (sensor_pose_publisher_) { + boost::shared_ptr baro = boost::static_pointer_cast(pose_estimation_->getMeasurement("baro")); + sensor_pose_.pose.position.z = baro->getModel()->getAltitude(Baro::Update(altimeter->pressure, altimeter->qnh)) - baro->getElevation(); + } +} + +} // namespace hector_quadrotor_pose_estimation diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_nodelet.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_nodelet.cpp new file mode 100644 index 0000000..928132e --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/src/pose_estimation_nodelet.cpp @@ -0,0 +1,58 @@ +//================================================================================================= +// Copyright (c) 2011, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#include +#include + +namespace hector_quadrotor_pose_estimation { + +class QuadrotorPoseEstimationNodelet : public QuadrotorPoseEstimationNode, public nodelet::Nodelet +{ +public: + QuadrotorPoseEstimationNodelet(const SystemPtr& system = SystemPtr()) + : QuadrotorPoseEstimationNode(system) + {} + +private: + void onInit() { + QuadrotorPoseEstimationNode::init(); + } + + void onReset() { + QuadrotorPoseEstimationNode::reset(); + } + + void onCleanup() { + QuadrotorPoseEstimationNode::cleanup(); + } +}; + +} // namespace hector_quadrotor_pose_estimation + +#include +PLUGINLIB_EXPORT_CLASS(hector_quadrotor_pose_estimation::QuadrotorPoseEstimationNodelet, nodelet::Nodelet) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CHANGELOG.rst new file mode 100644 index 0000000..404258e --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CHANGELOG.rst @@ -0,0 +1,29 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_quadrotor_teleop +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ +* Add optional parameter to set the joystick device +* Contributors: whoenig + +0.3.3 (2014-09-01) +------------------ +* added run dependency to joy package (joystick drivers) +* Contributors: Johannes Meyer + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ +* added slow button (btn 6 on Logitech Gamepad) +* Contributors: Johannes Meyer + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack +* decreased z_velocity_max to 2.0 m/s in logitech_gamepad.launch diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CMakeLists.txt new file mode 100644 index 0000000..0d437a2 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_quadrotor_teleop) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs hector_uav_msgs) +include_directories(include ${catkin_INCLUDE_DIRS}) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + LIBRARIES + CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs hector_uav_msgs + DEPENDS +) + +########### +## Build ## +########### + +## Declare a cpp library +# add_library(@{name} +# src/${PROJECT_NAME}/@name.cpp +# ) + +## Declare a cpp executable +add_executable(quadrotor_teleop src/quadrotor_teleop.cpp) +target_link_libraries(quadrotor_teleop ${catkin_LIBRARIES}) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(quadrotor_teleop ${catkin_EXPORTED_TARGETS}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS quadrotor_teleop + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/logitech_gamepad.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/logitech_gamepad.launch new file mode 100644 index 0000000..83884da --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/logitech_gamepad.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/xbox_controller.launch b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/xbox_controller.launch new file mode 100644 index 0000000..c6e9348 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/launch/xbox_controller.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/package.xml b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/package.xml new file mode 100644 index 0000000..88dfc1c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/package.xml @@ -0,0 +1,33 @@ + + hector_quadrotor_teleop + 0.3.5 + hector_quadrotor_teleop enables quadrotor flying with a joystick by + processing joy/Joy messages and translating them to geometry_msgs/Twist. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_quadrotor_teleop + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + + + catkin + + + roscpp + sensor_msgs + geometry_msgs + hector_uav_msgs + + + roscpp + sensor_msgs + geometry_msgs + hector_uav_msgs + joy + + + + diff --git a/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/src/quadrotor_teleop.cpp b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/src/quadrotor_teleop.cpp new file mode 100644 index 0000000..7d6e02e --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_quadrotor_teleop/src/quadrotor_teleop.cpp @@ -0,0 +1,215 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + + +#include +#include +#include +#include +#include +#include + +namespace hector_quadrotor +{ + +class Teleop +{ +private: + ros::NodeHandle node_handle_; + ros::Subscriber joy_subscriber_; + + ros::Publisher velocity_publisher_, attitude_publisher_, yawrate_publisher_, thrust_publisher_; + geometry_msgs::Twist velocity_; + hector_uav_msgs::AttitudeCommand attitude_; + hector_uav_msgs::ThrustCommand thrust_; + hector_uav_msgs::YawrateCommand yawrate_; + + struct Axis + { + int axis; + double max; + }; + + struct Button + { + int button; + }; + + struct + { + Axis x; + Axis y; + Axis z; + Axis yaw; + } axes_; + + struct + { + Button slow; + } buttons_; + + double slow_factor_; + +public: + Teleop() + { + ros::NodeHandle params("~"); + + params.param("x_axis", axes_.x.axis, 4); + params.param("y_axis", axes_.y.axis, 3); + params.param("z_axis", axes_.z.axis, 2); + params.param("yaw_axis", axes_.yaw.axis, 1); + + params.param("yaw_velocity_max", axes_.yaw.max, 90.0 * M_PI / 180.0); + params.param("slow_button", buttons_.slow.button, 1); + params.param("slow_factor", slow_factor_, 0.2); + + std::string control_mode_str; + params.param("control_mode", control_mode_str, "twist"); + + if (control_mode_str == "twist") + { + params.param("x_velocity_max", axes_.x.max, 2.0); + params.param("y_velocity_max", axes_.y.max, 2.0); + params.param("z_velocity_max", axes_.z.max, 2.0); + + joy_subscriber_ = node_handle_.subscribe("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1)); + velocity_publisher_ = node_handle_.advertise("cmd_vel", 10); + } + else if (control_mode_str == "attitude") + { + params.param("x_roll_max", axes_.x.max, 0.35); + params.param("y_pitch_max", axes_.y.max, 0.35); + params.param("z_thrust_max", axes_.z.max, 25.0); + joy_subscriber_ = node_handle_.subscribe("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1)); + attitude_publisher_ = node_handle_.advertise("command/attitude", 10); + yawrate_publisher_ = node_handle_.advertise("command/yawrate", 10); + thrust_publisher_ = node_handle_.advertise("command/thrust", 10); + } + + } + + ~Teleop() + { + stop(); + } + + void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy) + { + velocity_.linear.x = getAxis(joy, axes_.x); + velocity_.linear.y = getAxis(joy, axes_.y); + velocity_.linear.z = getAxis(joy, axes_.z); + velocity_.angular.z = getAxis(joy, axes_.yaw); + if (getButton(joy, buttons_.slow.button)) + { + velocity_.linear.x *= slow_factor_; + velocity_.linear.y *= slow_factor_; + velocity_.linear.z *= slow_factor_; + velocity_.angular.z *= slow_factor_; + } + velocity_publisher_.publish(velocity_); + } + + void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy) + { + attitude_.roll = getAxis(joy, axes_.x); + attitude_.pitch = getAxis(joy, axes_.y); + attitude_publisher_.publish(attitude_); + + thrust_.thrust = getAxis(joy, axes_.z); + thrust_publisher_.publish(thrust_); + + yawrate_.turnrate = getAxis(joy, axes_.yaw); + if (getButton(joy, buttons_.slow.button)) + { + yawrate_.turnrate *= slow_factor_; + } + yawrate_publisher_.publish(yawrate_); + } + + sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis) + { + if (axis.axis == 0) + {return 0;} + sensor_msgs::Joy::_axes_type::value_type sign = 1.0; + if (axis.axis < 0) + { + sign = -1.0; + axis.axis = -axis.axis; + } + if ((size_t) axis.axis > joy->axes.size()) + {return 0;} + return sign * joy->axes[axis.axis - 1] * axis.max; + } + + sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button) + { + if (button <= 0) + {return 0;} + if ((size_t) button > joy->buttons.size()) + {return 0;} + return joy->buttons[button - 1]; + } + + void stop() + { + if(velocity_publisher_.getNumSubscribers() > 0) + { + velocity_ = geometry_msgs::Twist(); + velocity_publisher_.publish(velocity_); + } + if(attitude_publisher_.getNumSubscribers() > 0) + { + attitude_ = hector_uav_msgs::AttitudeCommand(); + attitude_publisher_.publish(attitude_); + } + if(thrust_publisher_.getNumSubscribers() > 0) + { + thrust_ = hector_uav_msgs::ThrustCommand(); + thrust_publisher_.publish(thrust_); + } + if(yawrate_publisher_.getNumSubscribers() > 0) + { + yawrate_ = hector_uav_msgs::YawrateCommand(); + yawrate_publisher_.publish(yawrate_); + } + + } +}; + +} // namespace hector_quadrotor + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "quadrotor_teleop"); + + hector_quadrotor::Teleop teleop; + ros::spin(); + + return 0; +} diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/CHANGELOG.rst b/Flight_control/src/hector_quadrotor/hector_uav_msgs/CHANGELOG.rst new file mode 100644 index 0000000..1f783b0 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hector_uav_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.5 (2015-03-28) +------------------ + +0.3.4 (2015-02-22) +------------------ + +0.3.3 (2014-09-01) +------------------ +* updated value type in RawImu message to int16 +* added new ControllerState status constant FLYING +* added CLIMBRATE and TURNRATE constants to ControllerState message +* Contributors: Johannes Meyer + +0.3.2 (2014-03-30) +------------------ + +0.3.1 (2013-12-26) +------------------ + +0.3.0 (2013-09-11) +------------------ +* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/CMakeLists.txt b/Flight_control/src/hector_quadrotor/hector_uav_msgs/CMakeLists.txt new file mode 100644 index 0000000..dd9ac9e --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hector_uav_msgs) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED message_generation std_msgs) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +add_message_files( + FILES + Altimeter.msg + AttitudeCommand.msg + Compass.msg + ControllerState.msg + HeadingCommand.msg + HeightCommand.msg + MotorCommand.msg + MotorPWM.msg + MotorStatus.msg + PositionXYCommand.msg + RawImu.msg + RawMagnetic.msg + RawRC.msg + RC.msg + RuddersCommand.msg + ServoCommand.msg + Supply.msg + ThrustCommand.msg + VelocityXYCommand.msg + VelocityZCommand.msg + YawrateCommand.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES hector_uav_msgs + CATKIN_DEPENDS message_runtime std_msgs +# DEPENDS system_lib +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/Altimeter/pressure_height.h b/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/Altimeter/pressure_height.h new file mode 100644 index 0000000..6958138 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/Altimeter/pressure_height.h @@ -0,0 +1,61 @@ +//================================================================================================= +// Copyright (c) 2013, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_UAV_MSGS_ALTIMETER_PRESSURE_HEIGHT_H +#define HECTOR_UAV_MSGS_ALTIMETER_PRESSURE_HEIGHT_H + +#include +#include + +namespace hector_uav_msgs { + +static const Altimeter::_qnh_type STANDARD_PRESSURE = 1013.25; + +static inline Altimeter::_altitude_type altitudeFromPressure(Altimeter::_pressure_type pressure, Altimeter::_qnh_type qnh = STANDARD_PRESSURE) { + return 288.15 / 0.0065 * (1.0 - pow(pressure / qnh, 1.0/5.255)); +} + +static inline Altimeter::_pressure_type pressureFromAltitude(Altimeter::_altitude_type altitude, Altimeter::_qnh_type qnh = STANDARD_PRESSURE) { + return qnh * pow(1.0 - (0.0065 * altitude) / 288.15, 5.255); +} + +static inline Altimeter& altitudeFromPressure(Altimeter& altimeter) { + if (altimeter.qnh == 0.0) altimeter.qnh = STANDARD_PRESSURE; + altimeter.altitude = altitudeFromPressure(altimeter.pressure, altimeter.qnh); + return altimeter; +} + +static inline Altimeter& pressureFromAltitude(Altimeter& altimeter) { + if (altimeter.qnh == 0.0) altimeter.qnh = STANDARD_PRESSURE; + altimeter.pressure = pressureFromAltitude(altimeter.altitude, altimeter.qnh); + return altimeter; +} + +} // namespace hector_uav_msgs + +#endif // HECTOR_UAV_MSGS_ALTIMETER_PRESSURE_HEIGHT_H diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/ControlSource.h b/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/ControlSource.h new file mode 100644 index 0000000..bd4db2a --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/ControlSource.h @@ -0,0 +1,56 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_UAV_MSGS_CONTROLSOURCE_H +#define HECTOR_UAV_MSGS_CONTROLSOURCE_H + +namespace hector_uav_msgs +{ + typedef uint8_t ControlSource; + + enum { + CONTROL_AUTONOMOUS = 0, + CONTROL_REMOTE = 1, + CONTROL_JOYSTICK = 2 + }; + + template + static inline InStream& operator>>(InStream& in, ControlSource& value) { + int temp; + in >> temp; + value = static_cast(temp); + return in; + } + + template + static inline OutStream& operator<<(OutStream& out, const ControlSource& value) { + return out << static_cast(value); + } +} + +#endif // HECTOR_UAV_MSGS_CONTROLSOURCE_H diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/RC/functions.h b/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/RC/functions.h new file mode 100644 index 0000000..82c0df3 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/include/hector_uav_msgs/RC/functions.h @@ -0,0 +1,103 @@ +//================================================================================================= +// Copyright (c) 2012, Johannes Meyer, TU Darmstadt +// All rights reserved. + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of the Flight Systems and Automatic Control group, +// TU Darmstadt, nor the names of its contributors may be used to +// endorse or promote products derived from this software without +// specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//================================================================================================= + +#ifndef HECTOR_UAV_MSGS_RC_FUNCTIONS_H +#define HECTOR_UAV_MSGS_RC_FUNCTIONS_H + +#include +#include + +namespace hector_uav_msgs { + + static inline const char *getFunctionString(uint8_t function) + { + switch(function) { + case RC::ROLL: return "ROLL"; + case RC::PITCH: return "PITCH"; + case RC::YAW: return "YAW"; + case RC::STEER: return "STEER"; + case RC::HEIGHT: return "HEIGHT"; + case RC::THRUST: return "THRUST"; + case RC::BRAKE: return "BRAKE"; + } + return 0; + } + + static inline bool hasAxis(const RC& rc, RC::_axis_function_type::value_type function) + { + return std::find(rc.axis_function.begin(), rc.axis_function.end(), function) != rc.axis_function.end(); + } + + static inline bool getAxis(const RC& rc, RC::_axis_function_type::value_type function, RC::_axis_type::value_type& value) + { + if (!rc.valid) return false; + const RC::_axis_function_type::const_iterator it = std::find(rc.axis_function.begin(), rc.axis_function.end(), function); + if (it == rc.axis_function.end()) return false; + value = rc.axis.at(it - rc.axis_function.begin()); + return true; + } + + static inline void setAxis(RC& rc, RC::_axis_function_type::value_type function, RC::_axis_type::value_type value) + { + const RC::_axis_function_type::iterator it = std::find(rc.axis_function.begin(), rc.axis_function.end(), function); + if (it == rc.axis_function.end()) { + rc.axis_function.push_back(function); + rc.axis.push_back(value); + } else { + rc.axis.at(it - rc.axis_function.begin()) = value; + } + } + + static inline bool hasSwitch(const RC& rc, RC::_swit_function_type::value_type function) + { + return std::find(rc.swit_function.begin(), rc.swit_function.end(), function) != rc.swit_function.end(); + } + + static inline bool getSwitch(const RC& rc, RC::_swit_function_type::value_type function, RC::_swit_type::value_type& value) + { + if (!rc.valid) return false; + const RC::_swit_function_type::const_iterator it = std::find(rc.swit_function.begin(), rc.swit_function.end(), function); + if (it == rc.swit_function.end()) return false; + value = rc.swit.at(it - rc.swit_function.begin()); + return true; + } + + static inline void setSwitch(RC& rc, RC::_swit_function_type::value_type function, RC::_swit_type::value_type value) + { + const RC::_swit_function_type::iterator it = std::find(rc.swit_function.begin(), rc.swit_function.end(), function); + if (it == rc.swit_function.end()) { + rc.swit_function.push_back(function); + rc.swit.push_back(value); + } else { + rc.swit.at(it - rc.swit_function.begin()) = value; + } + } + +} // namespace hector_uav_msgs + +#endif // HECTOR_UAV_MSGS_RC_FUNCTIONS_H diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Altimeter.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Altimeter.msg new file mode 100644 index 0000000..20b6a3c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Altimeter.msg @@ -0,0 +1,4 @@ +Header header +float32 altitude +float32 pressure +float32 qnh diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/AttitudeCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/AttitudeCommand.msg new file mode 100644 index 0000000..d63fb89 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/AttitudeCommand.msg @@ -0,0 +1,3 @@ +Header header +float32 roll +float32 pitch diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Compass.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Compass.msg new file mode 100644 index 0000000..d2bb3fe --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Compass.msg @@ -0,0 +1,4 @@ +Header header +float32 magnetic_heading +float32 declination + diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ControllerState.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ControllerState.msg new file mode 100644 index 0000000..9903ca8 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ControllerState.msg @@ -0,0 +1,17 @@ +Header header +uint8 source + +uint8 mode +uint8 MOTORS = 1 +uint8 ATTITUDE = 2 +uint8 VELOCITY = 4 +uint8 POSITION = 8 +uint8 TURNRATE = 16 +uint8 HEADING = 32 +uint8 CLIMBRATE = 64 +uint8 HEIGHT = 128 + +uint8 state +uint8 MOTORS_RUNNING = 1 +uint8 FLYING = 2 +uint8 AIRBORNE = 4 diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeadingCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeadingCommand.msg new file mode 100644 index 0000000..5ad41a3 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeadingCommand.msg @@ -0,0 +1,2 @@ +Header header +float32 heading diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeightCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeightCommand.msg new file mode 100644 index 0000000..0d64519 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/HeightCommand.msg @@ -0,0 +1,2 @@ +Header header +float32 height diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorCommand.msg new file mode 100644 index 0000000..10ed1db --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorCommand.msg @@ -0,0 +1,5 @@ +Header header +float32[] force +float32[] torque +float32[] frequency +float32[] voltage diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorPWM.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorPWM.msg new file mode 100644 index 0000000..d2bf65f --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorPWM.msg @@ -0,0 +1,2 @@ +Header header +uint8[] pwm diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorStatus.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorStatus.msg new file mode 100644 index 0000000..9c09473 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/MotorStatus.msg @@ -0,0 +1,7 @@ +Header header +bool on +bool running +float32[] voltage +float32[] frequency +float32[] current + diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/PositionXYCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/PositionXYCommand.msg new file mode 100644 index 0000000..de72e09 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/PositionXYCommand.msg @@ -0,0 +1,3 @@ +Header header +float32 x +float32 y diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RC.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RC.msg new file mode 100644 index 0000000..8dfdc55 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RC.msg @@ -0,0 +1,18 @@ +Header header + +uint8 ROLL = 1 +uint8 PITCH = 2 +uint8 YAW = 3 +uint8 STEER = 4 +uint8 HEIGHT = 5 +uint8 THRUST = 6 +uint8 BRAKE = 7 + +uint8 status +bool valid + +float32[] axis +uint8[] axis_function + +int8[] swit +uint8[] swit_function diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawImu.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawImu.msg new file mode 100644 index 0000000..21c29a2 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawImu.msg @@ -0,0 +1,3 @@ +Header header +int16[3] angular_velocity +int16[3] linear_acceleration diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawMagnetic.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawMagnetic.msg new file mode 100644 index 0000000..0a2543c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawMagnetic.msg @@ -0,0 +1,2 @@ +Header header +float64[3] channel diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawRC.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawRC.msg new file mode 100644 index 0000000..52b397c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RawRC.msg @@ -0,0 +1,3 @@ +Header header +uint8 status +uint16[] channel diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RuddersCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RuddersCommand.msg new file mode 100644 index 0000000..24a2773 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/RuddersCommand.msg @@ -0,0 +1,4 @@ +Header header +float32 aileron +float32 elevator +float32 rudder diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ServoCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ServoCommand.msg new file mode 100644 index 0000000..91b661c --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ServoCommand.msg @@ -0,0 +1,2 @@ +Header header +uint16[] value diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Supply.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Supply.msg new file mode 100644 index 0000000..ca08cd6 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/Supply.msg @@ -0,0 +1,3 @@ +Header header +float32[] voltage +float32[] current diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ThrustCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ThrustCommand.msg new file mode 100644 index 0000000..fefdb9e --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/ThrustCommand.msg @@ -0,0 +1,2 @@ +Header header +float32 thrust diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityXYCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityXYCommand.msg new file mode 100644 index 0000000..de72e09 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityXYCommand.msg @@ -0,0 +1,3 @@ +Header header +float32 x +float32 y diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityZCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityZCommand.msg new file mode 100644 index 0000000..0910464 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/VelocityZCommand.msg @@ -0,0 +1,2 @@ +Header header +float32 z diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/YawrateCommand.msg b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/YawrateCommand.msg new file mode 100644 index 0000000..fd2de10 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/msg/YawrateCommand.msg @@ -0,0 +1,2 @@ +Header header +float32 turnrate diff --git a/Flight_control/src/hector_quadrotor/hector_uav_msgs/package.xml b/Flight_control/src/hector_quadrotor/hector_uav_msgs/package.xml new file mode 100644 index 0000000..9042717 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/hector_uav_msgs/package.xml @@ -0,0 +1,27 @@ + + hector_uav_msgs + 0.3.5 + 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. + Johannes Meyer + + BSD + + http://ros.org/wiki/hector_uav_msgs + https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues + + Johannes Meyer + + + catkin + + + message_generation + std_msgs + + + message_runtime + std_msgs + + + + diff --git a/Flight_control/src/hector_quadrotor/tutorials.rosinstall b/Flight_control/src/hector_quadrotor/tutorials.rosinstall new file mode 100644 index 0000000..bdfce35 --- /dev/null +++ b/Flight_control/src/hector_quadrotor/tutorials.rosinstall @@ -0,0 +1,5 @@ +- git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: indigo-devel} +- git: {local-name: hector_slam, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_slam.git', version: catkin} +- git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin} +- git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: indigo-devel} +- git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: indigo-devel}