├── .gitignore ├── .start_test_uav1_with_downward_cam.sh.swp ├── ReadMe.md ├── catkin_ws ├── battle.sh ├── kill.sh ├── load_bullet.sh ├── python_kill.sh ├── reset.sh ├── scripts │ ├── initial_pose │ ├── load_bullet.sh │ ├── reset_to_initial_pose.sh │ ├── uav_arm.sh │ ├── uav_arm.sh.bak │ └── uav_disarm.sh └── src │ ├── QRtags_imu_navigation │ ├── apriltag_ros │ │ ├── LICENSE │ │ ├── README.md │ │ ├── apriltag │ │ │ ├── .travis.yml │ │ │ ├── CMakeLists.txt │ │ │ ├── LICENSE.md │ │ │ ├── Makefile │ │ │ ├── README.md │ │ │ ├── apriltag.c │ │ │ ├── apriltag.h │ │ │ ├── apriltag.pc.in │ │ │ ├── apriltag_detect.docstring │ │ │ ├── apriltag_math.h │ │ │ ├── apriltag_pose.c │ │ │ ├── apriltag_pose.h │ │ │ ├── apriltag_py_type.docstring │ │ │ ├── apriltag_pywrap.c │ │ │ ├── apriltag_quad_thresh.c │ │ │ ├── common │ │ │ │ ├── doubles.h │ │ │ │ ├── doubles_floats_impl.h │ │ │ │ ├── floats.h │ │ │ │ ├── g2d.c │ │ │ │ ├── g2d.h │ │ │ │ ├── getopt.c │ │ │ │ ├── getopt.h │ │ │ │ ├── homography.c │ │ │ │ ├── homography.h │ │ │ │ ├── image_types.h │ │ │ │ ├── image_u8.c │ │ │ │ ├── image_u8.h │ │ │ │ ├── image_u8x3.c │ │ │ │ ├── image_u8x3.h │ │ │ │ ├── image_u8x4.c │ │ │ │ ├── image_u8x4.h │ │ │ │ ├── matd.c │ │ │ │ ├── matd.h │ │ │ │ ├── math_util.h │ │ │ │ ├── pam.c │ │ │ │ ├── pam.h │ │ │ │ ├── pjpeg-idct.c │ │ │ │ ├── pjpeg.c │ │ │ │ ├── pjpeg.h │ │ │ │ ├── pnm.c │ │ │ │ ├── pnm.h │ │ │ │ ├── postscript_utils.h │ │ │ │ ├── string_util.c │ │ │ │ ├── string_util.h │ │ │ │ ├── svd22.c │ │ │ │ ├── svd22.h │ │ │ │ ├── time_util.c │ │ │ │ ├── time_util.h │ │ │ │ ├── timeprofile.h │ │ │ │ ├── unionfind.c │ │ │ │ ├── unionfind.h │ │ │ │ ├── workerpool.c │ │ │ │ ├── workerpool.h │ │ │ │ ├── zarray.c │ │ │ │ ├── zarray.h │ │ │ │ ├── zhash.c │ │ │ │ ├── zhash.h │ │ │ │ ├── zmaxheap.c │ │ │ │ └── zmaxheap.h │ │ │ ├── example │ │ │ │ ├── .gitignore │ │ │ │ ├── Makefile │ │ │ │ ├── README │ │ │ │ ├── apriltag_demo.c │ │ │ │ └── opencv_demo.cc │ │ │ ├── install.sh │ │ │ ├── package.xml │ │ │ ├── python_build_flags.py │ │ │ ├── tag16h5.c │ │ │ ├── tag16h5.h │ │ │ ├── tag25h9.c │ │ │ ├── tag25h9.h │ │ │ ├── tag36h11.c │ │ │ ├── tag36h11.h │ │ │ ├── tagCircle21h7.c │ │ │ ├── tagCircle21h7.h │ │ │ ├── tagCircle49h12.c │ │ │ ├── tagCircle49h12.h │ │ │ ├── tagCustom48h12.c │ │ │ ├── tagCustom48h12.h │ │ │ ├── tagStandard41h12.c │ │ │ ├── tagStandard41h12.h │ │ │ ├── tagStandard52h13.c │ │ │ └── tagStandard52h13.h │ │ └── apriltag_ros │ │ │ ├── CHANGELOG.rst │ │ │ ├── CMakeLists.txt │ │ │ ├── config │ │ │ ├── settings.yaml │ │ │ └── tags.yaml │ │ │ ├── include │ │ │ └── apriltag_ros │ │ │ │ ├── common_functions.h │ │ │ │ ├── continuous_detector.h │ │ │ │ └── single_image_detector.h │ │ │ ├── launch │ │ │ ├── continuous_detection.launch │ │ │ ├── single_image_client.launch │ │ │ └── single_image_server.launch │ │ │ ├── msg │ │ │ ├── AprilTagDetection.msg │ │ │ └── AprilTagDetectionArray.msg │ │ │ ├── nodelet_plugins.xml │ │ │ ├── package.xml │ │ │ ├── scripts │ │ │ ├── analyze_image │ │ │ └── calibrate_bundle.m │ │ │ ├── src │ │ │ ├── apriltag_ros_continuous_node.cpp │ │ │ ├── apriltag_ros_single_image_client_node.cpp │ │ │ ├── apriltag_ros_single_image_server_node.cpp │ │ │ ├── common_functions.cpp │ │ │ ├── continuous_detector.cpp │ │ │ └── single_image_detector.cpp │ │ │ └── srv │ │ │ └── AnalyzeSingleImage.srv │ ├── integrated_navigation │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── integrated_navigation_node.launch │ │ ├── package.xml │ │ └── scripts │ │ │ ├── imu_processing.py │ │ │ ├── prepare_tags_info_node.py │ │ │ ├── tools.py │ │ │ └── tools.pyc │ └── robot_localization │ │ ├── .gitignore │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── LICENSE │ │ ├── README.md │ │ ├── doc │ │ ├── .templates │ │ │ └── full_globaltoc.html │ │ ├── CHANGELOG.rst │ │ ├── Makefile │ │ ├── conf.py │ │ ├── configuring_robot_localization.rst │ │ ├── images │ │ │ ├── figure1.png │ │ │ └── rl_small.png │ │ ├── index.rst │ │ ├── integrating_gps.rst │ │ ├── manifest.yaml │ │ ├── migrating_from_robot_pose_ekf.rst │ │ ├── navsat_transform_node.rst │ │ ├── preparing_sensor_data.rst │ │ ├── robot_localization_ias13_revised.pdf │ │ ├── state_estimation_nodes.rst │ │ └── user_contributed_tutorials.rst │ │ ├── include │ │ └── robot_localization │ │ │ ├── ekf.h │ │ │ ├── filter_base.h │ │ │ ├── filter_common.h │ │ │ ├── filter_utilities.h │ │ │ ├── navsat_conversions.h │ │ │ ├── navsat_transform.h │ │ │ ├── robot_localization_estimator.h │ │ │ ├── ros_filter.h │ │ │ ├── ros_filter_types.h │ │ │ ├── ros_filter_utilities.h │ │ │ ├── ros_robot_localization_listener.h │ │ │ └── ukf.h │ │ ├── launch │ │ ├── dual_ekf_navsat_example.launch │ │ ├── ekf_nodelet_template.launch │ │ ├── ekf_template.launch │ │ ├── navsat_transform_template.launch │ │ ├── tags_imu_ekf.launch │ │ └── ukf_template.launch │ │ ├── nodelet_plugins.xml │ │ ├── package.xml │ │ ├── params │ │ ├── dual_ekf_navsat_example.yaml │ │ ├── ekf_template.yaml │ │ ├── navsat_transform_template.yaml │ │ ├── tags_imu_ekf.yaml │ │ └── ukf_template.yaml │ │ ├── rosdoc.yaml │ │ ├── src │ │ ├── ekf.cpp │ │ ├── ekf_localization_node.cpp │ │ ├── ekf_localization_nodelet.cpp │ │ ├── filter_base.cpp │ │ ├── filter_utilities.cpp │ │ ├── navsat_transform.cpp │ │ ├── navsat_transform_node.cpp │ │ ├── navsat_transform_nodelet.cpp │ │ ├── robot_localization_estimator.cpp │ │ ├── robot_localization_listener_node.cpp │ │ ├── ros_filter.cpp │ │ ├── ros_filter_utilities.cpp │ │ ├── ros_robot_localization_listener.cpp │ │ ├── ukf.cpp │ │ ├── ukf_localization_node.cpp │ │ └── ukf_localization_nodelet.cpp │ │ ├── srv │ │ ├── FromLL.srv │ │ ├── GetState.srv │ │ ├── SetDatum.srv │ │ ├── SetPose.srv │ │ ├── ToLL.srv │ │ └── ToggleFilterProcessing.srv │ │ └── test │ │ ├── record_all_stats.sh │ │ ├── stat_recorder.sh │ │ ├── test1.bag │ │ ├── test2.bag │ │ ├── test3.bag │ │ ├── test_ekf.cpp │ │ ├── test_ekf.test │ │ ├── test_ekf_localization_node_bag1.test │ │ ├── test_ekf_localization_node_bag2.test │ │ ├── test_ekf_localization_node_bag3.test │ │ ├── test_ekf_localization_node_interfaces.cpp │ │ ├── test_ekf_localization_node_interfaces.test │ │ ├── test_ekf_localization_nodelet_bag1.test │ │ ├── test_filter_base.cpp │ │ ├── test_filter_base_diagnostics_timestamps.cpp │ │ ├── test_filter_base_diagnostics_timestamps.test │ │ ├── test_localization_node_bag_pose_tester.cpp │ │ ├── test_robot_localization_estimator.cpp │ │ ├── test_robot_localization_estimator.test │ │ ├── test_ros_robot_localization_listener.cpp │ │ ├── test_ros_robot_localization_listener.test │ │ ├── test_ros_robot_localization_listener_publisher.cpp │ │ ├── test_ukf.cpp │ │ ├── test_ukf.test │ │ ├── test_ukf_localization_node_bag1.test │ │ ├── test_ukf_localization_node_bag2.test │ │ ├── test_ukf_localization_node_bag3.test │ │ ├── test_ukf_localization_node_interfaces.cpp │ │ ├── test_ukf_localization_node_interfaces.test │ │ └── test_ukf_localization_nodelet_bag1.test │ ├── bullet_control │ ├── CMakeLists.txt │ ├── cmd │ ├── launch │ │ └── load_bullet.launch │ ├── package.xml │ ├── sdf │ │ └── my_bullet_control │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── src │ │ ├── bullet_plugin.cc │ │ └── vel.cc │ ├── urdf │ │ └── model.urdf │ └── world │ │ └── bullet.world │ ├── hector_quad_simple_ctrl │ ├── CMakeLists.txt │ ├── include │ │ └── authorized_check.h │ ├── launch │ │ ├── hector_ctrl_test.launch │ │ ├── hector_pose_ctrl.launch │ │ └── hector_twist_ctrl.launch │ ├── package.xml │ ├── scripts │ │ ├── pub_multiple_pose.py │ │ ├── pub_pose.py │ │ ├── send_pose_cmd_static.py │ │ ├── takeoff_land_node.py │ │ └── test_pose_cmd.py │ ├── sh_scripts │ │ ├── pub_vel_uav1.sh │ │ ├── start_uav1.sh │ │ ├── stop_uav1.sh │ │ ├── test_cmd.sh │ │ └── uav1_takeoff │ └── src │ │ ├── authorized_check.cpp │ │ ├── hector_pose_ctrl_node.cpp │ │ ├── hector_pose_ctrl_node_old.cpp │ │ ├── hector_twist_ctrl_node.cpp │ │ └── pos_controller.cpp │ ├── hector_quadrotor │ ├── LICENSE.txt │ ├── hector_gazebo_plugins │ │ ├── .gitignore │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── cfg │ │ │ ├── GNSS.cfg │ │ │ └── SensorModel.cfg │ │ ├── include │ │ │ └── hector_gazebo_plugins │ │ │ │ ├── diffdrive_plugin_6w.h │ │ │ │ ├── diffdrive_plugin_multi_wheel.h │ │ │ │ ├── gazebo_ros_force_based_move.h │ │ │ │ ├── gazebo_ros_gps.h │ │ │ │ ├── gazebo_ros_imu.h │ │ │ │ ├── gazebo_ros_magnetic.h │ │ │ │ ├── gazebo_ros_sonar.h │ │ │ │ ├── reset_plugin.h │ │ │ │ ├── sensor_model.h │ │ │ │ ├── servo_plugin.h │ │ │ │ └── update_timer.h │ │ ├── package.xml │ │ ├── src │ │ │ ├── diffdrive_plugin_6w.cpp │ │ │ ├── diffdrive_plugin_multi_wheel.cpp │ │ │ ├── gazebo_ros_force_based_move.cpp │ │ │ ├── gazebo_ros_gps.cpp │ │ │ ├── gazebo_ros_imu.cpp │ │ │ ├── gazebo_ros_magnetic.cpp │ │ │ ├── gazebo_ros_sonar.cpp │ │ │ ├── reset_plugin.cpp │ │ │ └── servo_plugin.cpp │ │ └── srv │ │ │ └── SetBias.srv │ ├── hector_quadrotor.rosinstall │ ├── hector_quadrotor │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml │ ├── hector_quadrotor_actions │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── hector_quadrotor_actions │ │ │ │ └── base_action.h │ │ ├── launch │ │ │ └── actions.launch │ │ ├── package.xml │ │ └── src │ │ │ ├── landing_action.cpp │ │ │ ├── pose_action.cpp │ │ │ └── takeoff_action.cpp │ ├── hector_quadrotor_controller_gazebo │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── hector_quadrotor_controller_gazebo │ │ │ │ └── quadrotor_hardware_gazebo.h │ │ ├── package.xml │ │ ├── quadrotor_controller_gazebo.xml │ │ └── src │ │ │ └── quadrotor_hardware_gazebo.cpp │ ├── hector_quadrotor_controllers │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── controller.launch │ │ ├── package.xml │ │ ├── params │ │ │ ├── controller.yaml │ │ │ └── params.yaml │ │ ├── plugin.xml │ │ └── src │ │ │ ├── attitude_controller.cpp │ │ │ ├── position_controller.cpp │ │ │ └── velocity_controller.cpp │ ├── hector_quadrotor_demo │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── indoor_slam_gazebo.launch │ │ │ └── outdoor_flight_gazebo.launch │ │ ├── package.xml │ │ └── rviz_cfg │ │ │ ├── indoor_slam.rviz │ │ │ └── outdoor_flight.rviz │ ├── hector_quadrotor_description │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── xacrodisplay_quadrotor_base.launch │ │ ├── meshes │ │ │ └── quadrotor │ │ │ │ ├── blender │ │ │ │ └── quadrotor_base.blend │ │ │ │ ├── quadrotor_base.dae │ │ │ │ ├── quadrotor_base.stl │ │ │ │ ├── quadrotor_base_blue.dae │ │ │ │ └── quadrotor_base_red.dae │ │ ├── package.xml │ │ └── urdf │ │ │ ├── blue │ │ │ ├── quadrotor_base_blue.urdf.xacro │ │ │ ├── quadrotor_blue.gazebo.xacro │ │ │ ├── quadrotor_blue.urdf.xacro │ │ │ ├── quadrotor_with_cam_blue.gazebo.xacro │ │ │ └── quadrotor_with_cam_blue.urdf.xacro │ │ │ ├── quadrotor.gazebo.xacro │ │ │ ├── quadrotor.urdf.xacro │ │ │ ├── quadrotor_base.urdf.xacro │ │ │ ├── quadrotor_downward_cam.gazebo.xacro │ │ │ ├── quadrotor_downward_cam.urdf.xacro │ │ │ ├── quadrotor_hokuyo_utm30lx.gazebo.xacro │ │ │ ├── quadrotor_hokuyo_utm30lx.urdf.xacro │ │ │ ├── quadrotor_with_asus.gazebo.xacro │ │ │ ├── quadrotor_with_asus.urdf.xacro │ │ │ ├── quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro │ │ │ ├── quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro │ │ │ ├── quadrotor_with_cam.gazebo.xacro │ │ │ ├── quadrotor_with_cam.urdf.xacro │ │ │ ├── quadrotor_with_kinect.gazebo.xacro │ │ │ ├── quadrotor_with_kinect.urdf.xacro │ │ │ └── red │ │ │ ├── quadrotor_base_red.urdf.xacro │ │ │ ├── quadrotor_red.gazebo.xacro │ │ │ ├── quadrotor_red.urdf.xacro │ │ │ ├── quadrotor_with_cam_red.gazebo.xacro │ │ │ └── quadrotor_with_cam_red.urdf.xacro │ ├── hector_quadrotor_gazebo │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── gen_spawn_100.launch │ │ │ ├── gen_spawn_4.launch │ │ │ ├── gen_spawn_40.launch │ │ │ ├── gen_spawn_64.launch │ │ │ ├── quadrotor_empty_world.launch │ │ │ ├── quadrotor_empty_world_test.launch │ │ │ ├── sim_100_drones_world.launch │ │ │ ├── sim_40_drones_world.launch │ │ │ ├── sim_ten_drones_world.launch │ │ │ ├── spawn_four_quadrotors.launch │ │ │ ├── spawn_one_quadrotors.launch │ │ │ ├── spawn_one_quadrotors_for_maze.launch │ │ │ ├── spawn_one_quadrotors_for_maze_hard.launch │ │ │ ├── spawn_quadrotor.launch │ │ │ ├── spawn_quadrotor_with_cam.launch │ │ │ ├── spawn_quadrotor_with_downward_cam.launch │ │ │ ├── spawn_quadrotor_with_laser.launch │ │ │ └── spawn_ten_quadrotors.launch │ │ ├── package.xml │ │ ├── urdf │ │ │ ├── CMakeLists.txt │ │ │ ├── box.urdf │ │ │ ├── quadrotor_aerodynamics.gazebo.xacro │ │ │ ├── quadrotor_controller.gazebo.xacro │ │ │ ├── quadrotor_plugins.gazebo.xacro │ │ │ ├── quadrotor_plugins.gazebo.xacro.in │ │ │ ├── quadrotor_propulsion.gazebo.xacro │ │ │ └── quadrotor_sensors.gazebo.xacro │ │ └── worlds │ │ │ ├── gimbel_demo.world │ │ │ ├── lmx.world │ │ │ └── lmx22gzl.world │ ├── hector_quadrotor_gazebo_plugins │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── hector_quadrotor_gazebo_plugins │ │ │ │ ├── gazebo_quadrotor_aerodynamics.h │ │ │ │ ├── gazebo_quadrotor_propulsion.h │ │ │ │ ├── gazebo_quadrotor_simple_controller.h │ │ │ │ └── gazebo_ros_baro.h │ │ ├── package.xml │ │ └── src │ │ │ ├── gazebo_quadrotor_aerodynamics.cpp │ │ │ ├── gazebo_quadrotor_propulsion.cpp │ │ │ ├── gazebo_quadrotor_simple_controller.cpp │ │ │ └── gazebo_ros_baro.cpp │ ├── hector_quadrotor_interface │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── hector_quadrotor_interface │ │ │ │ ├── handles.h │ │ │ │ ├── helpers.h │ │ │ │ ├── limiters.h │ │ │ │ └── quadrotor_interface.h │ │ ├── package.xml │ │ └── src │ │ │ ├── helpers.cpp │ │ │ └── quadrotor_interface.cpp │ ├── hector_quadrotor_model │ │ ├── .gitignore │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── hector_quadrotor_model │ │ │ │ ├── helpers.h │ │ │ │ ├── quadrotor_aerodynamics.h │ │ │ │ └── quadrotor_propulsion.h │ │ ├── matlab │ │ │ ├── CMakeLists.txt │ │ │ ├── compile_all.m │ │ │ ├── dummy.c │ │ │ ├── motorspeed.m │ │ │ ├── parameter_QK_propulsion.m │ │ │ ├── quadrotorDrag.m │ │ │ └── quadrotorPropulsion.m │ │ ├── package.xml │ │ ├── param │ │ │ ├── quadrotor_aerodynamics.yaml │ │ │ ├── robbe_2827-34_epp1045.yaml │ │ │ └── robbe_2827-34_epp1245.yaml │ │ └── src │ │ │ ├── matlab_helpers.h │ │ │ ├── quadrotor_aerodynamics.cpp │ │ │ └── quadrotor_propulsion.cpp │ ├── hector_quadrotor_pose_estimation │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── hector_quadrotor_pose_estimation_nodelets.xml │ │ ├── include │ │ │ └── hector_quadrotor_pose_estimation │ │ │ │ └── pose_estimation_node.h │ │ ├── package.xml │ │ ├── params │ │ │ └── simulation.yaml │ │ └── src │ │ │ ├── main.cpp │ │ │ ├── pose_estimation_node.cpp │ │ │ └── pose_estimation_nodelet.cpp │ ├── hector_quadrotor_teleop │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── logitech_gamepad.launch │ │ │ ├── sony_dualshock3.launch │ │ │ └── xbox_controller.launch │ │ ├── package.xml │ │ └── src │ │ │ └── quadrotor_teleop.cpp │ ├── hector_uav_msgs │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── action │ │ │ ├── Landing.action │ │ │ ├── Pose.action │ │ │ └── Takeoff.action │ │ ├── include │ │ │ └── hector_uav_msgs │ │ │ │ ├── Altimeter │ │ │ │ └── pressure_height.h │ │ │ │ ├── ControlSource.h │ │ │ │ └── RC │ │ │ │ └── functions.h │ │ ├── msg │ │ │ ├── Altimeter.msg │ │ │ ├── AttitudeCommand.msg │ │ │ ├── Compass.msg │ │ │ ├── ControllerState.msg │ │ │ ├── HeadingCommand.msg │ │ │ ├── HeightCommand.msg │ │ │ ├── MotorCommand.msg │ │ │ ├── MotorPWM.msg │ │ │ ├── MotorStatus.msg │ │ │ ├── PositionXYCommand.msg │ │ │ ├── RC.msg │ │ │ ├── RawImu.msg │ │ │ ├── RawMagnetic.msg │ │ │ ├── RawRC.msg │ │ │ ├── RuddersCommand.msg │ │ │ ├── ServoCommand.msg │ │ │ ├── Supply.msg │ │ │ ├── ThrustCommand.msg │ │ │ ├── VelocityXYCommand.msg │ │ │ ├── VelocityZCommand.msg │ │ │ └── YawrateCommand.msg │ │ ├── package.xml │ │ └── srv │ │ │ └── EnableMotors.srv │ └── tutorials.rosinstall │ ├── innok_heros_description │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── launch │ │ ├── innok_heros.rviz │ │ └── innok_heros_rviz.launch │ ├── meshes │ │ ├── base_link.STL │ │ ├── battery_box.STL │ │ ├── beam.STL │ │ ├── caster_bot.STL │ │ ├── caster_top.STL │ │ ├── caster_wheel.STL │ │ ├── drive_box.STL │ │ ├── mounting_plate_3w.STL │ │ ├── mounting_plate_4w.STL │ │ ├── swing_axle_bot.STL │ │ ├── swing_axle_top.STL │ │ ├── wheel.STL │ │ ├── wheel_tractor.STL │ │ └── wheel_tractor_large.STL │ ├── package.xml │ └── urdf │ │ ├── ROS │ │ ├── heros_diff_drive.xacro │ │ ├── heros_joint_pub.xacro │ │ └── heros_skid_drive.xacro │ │ ├── blue │ │ ├── innok_heros_4wtractor_blue.xacro │ │ └── innok_heros_4wtractor_with_cam_blue.xacro │ │ ├── innok_heros_3w.xacro │ │ ├── innok_heros_3wtractor.xacro │ │ ├── innok_heros_4w.xacro │ │ ├── innok_heros_4wtractor.xacro │ │ ├── modules │ │ ├── heros_base_link.xacro │ │ ├── heros_battery.xacro │ │ ├── heros_caster.xacro │ │ ├── heros_drive.xacro │ │ ├── heros_mounting_plate.xacro │ │ ├── heros_mounting_plate_blue.xacro │ │ ├── heros_mounting_plate_red.xacro │ │ ├── heros_swing_axle.xacro │ │ ├── heros_wheel.xacro │ │ ├── heros_wheel_tractor.xacro │ │ ├── heros_wheel_tractor_blue.xacro │ │ ├── heros_wheel_tractor_large.xacro │ │ └── heros_wheel_tractor_red.xacro │ │ ├── red │ │ ├── innok_heros_4wtractor_red.xacro │ │ └── innok_heros_4wtractor_with_cam_red.xacro │ │ └── swing_axle_urdf_package.urdf │ ├── innok_heros_gazebo │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── building_editor_models │ │ └── lmx2Y2B │ │ │ ├── 19052514A_Map1 │ │ │ ├── 19052514CenterCylin │ │ │ ├── model.config │ │ │ └── model.sdf │ │ │ ├── 19052514EdgeWall │ │ │ ├── model.config │ │ │ └── model.sdf │ │ │ ├── 19052514EdgeWall_2m │ │ │ ├── model.config │ │ │ └── model.sdf │ │ │ ├── 19052618A_Map2 │ │ │ ├── 19052618A_Map2_testtexture │ │ │ └── head_texture.png │ ├── launch │ │ ├── A │ │ │ ├── cars_A2.launch │ │ │ ├── cars_A3.launch │ │ │ ├── cars_A5.launch │ │ │ ├── uavs_A1.launch │ │ │ ├── uavs_A10.launch │ │ │ └── uavs_A3.launch │ │ ├── B │ │ │ ├── cars_B2.launch │ │ │ ├── cars_B3.launch │ │ │ ├── cars_B5.launch │ │ │ ├── uavs_B10.launch │ │ │ └── uavs_B3.launch │ │ ├── cars_A_test_2.launch │ │ ├── cars_B_test_2.launch │ │ ├── load_cars10_uavs20.launch │ │ ├── load_cars4_uavs6.launch │ │ ├── load_cars6_uavs6.launch │ │ ├── load_hard_world.launch │ │ ├── load_maze_world.launch │ │ ├── load_uav_B5_A8.launch │ │ ├── load_world_1.launch │ │ ├── load_world_2.launch │ │ ├── load_world_3.launch │ │ ├── load_world_60x40.launch │ │ ├── load_world_apriltag_map.launch │ │ ├── spawn_innok_vehicle.launch │ │ ├── uavs_A8.launch │ │ └── uavs_B5.launch │ ├── package.xml │ └── worlds │ │ ├── 19052514A_Map1 │ │ ├── 19052514CenterCylin │ │ ├── model.config │ │ └── model.sdf │ │ ├── 19052514EdgeWall │ │ ├── model.config │ │ └── model.sdf │ │ ├── 19052514EdgeWall_2m │ │ ├── model.config │ │ └── model.sdf │ │ ├── 19052618A_Map2 │ │ ├── 19052618A_Map2.bak │ │ ├── 19052618A_Map2_testtexture │ │ ├── 1908082Y2B_simplesituation_color.world │ │ ├── 1908082Y2B_simplesituation_color2.world │ │ ├── 1908082Y2B_simplesituation_color3.world │ │ ├── 1908082Y2B_simplesituation_color4.world │ │ ├── Apriltags_world │ │ ├── README.md │ │ ├── apriltags_map.world │ │ └── models │ │ │ ├── Apriltags_5x5_plane.tar.gz │ │ │ ├── Apriltags_plane │ │ │ ├── materials │ │ │ │ ├── scripts │ │ │ │ │ └── Apriltag.material │ │ │ │ └── textures │ │ │ │ │ ├── apriltagMap_5x5_4.9000m.png │ │ │ │ │ ├── apriltagMap_7x7_6.7000m.png │ │ │ │ │ └── apriltagMap_9x9_8.5000m.png │ │ │ ├── model.config │ │ │ └── model.sdf │ │ │ ├── add_path.sh │ │ │ ├── apriltagMapInfo_5x5_4.9000m.txt │ │ │ ├── apriltagMapInfo_7x7_6.7000m.txt │ │ │ ├── apriltagMapInfo_9x9_8.5000m.txt │ │ │ ├── apriltagMap_5x5_4.9000m.png │ │ │ ├── apriltagMap_7x7_6.7000m.png │ │ │ └── apriltagMap_9x9_8.5000m.png │ │ ├── GroundControlStation_A_B.world │ │ ├── center_x7_y9_z7.5_60x40.world │ │ ├── fountain_60x40.world │ │ ├── head_texture.png │ │ ├── map1.world │ │ ├── maze_easy.world │ │ ├── maze_hard.world │ │ ├── maze_hard_ok.world │ │ └── maze_hard_white_bg.world │ ├── judge_system │ ├── CMakeLists.txt │ ├── include │ │ ├── authorized_check.h │ │ ├── judge_gui.h │ │ └── judge_interface.h │ ├── launch │ │ ├── judge_system_Buav5_Auav8.launch │ │ ├── judge_system_car4_uav6.launch │ │ └── judge_system_car6_uav6.launch │ ├── package.xml │ └── src │ │ ├── authorized_check.cc │ │ ├── judge_gui.cpp │ │ ├── judge_interface.cpp │ │ ├── main.cpp │ │ └── main_gui.cpp │ ├── judge_system_msgs │ ├── CMakeLists.txt │ ├── msg │ │ ├── Alive_model.msg │ │ ├── Alive_models.msg │ │ ├── Attack_cmd.msg │ │ ├── Bullet_launch.msg │ │ └── Move_cmd.msg │ ├── package.xml │ └── srv │ │ └── clear_bullets.srv │ ├── sc_gazebo │ ├── CMakeLists.txt │ ├── config │ │ ├── config.yaml │ │ ├── multiple_A3_B3_config.yaml │ │ ├── multiple_config.yaml │ │ ├── sc_config.yaml │ │ └── stone_v3_config.yaml │ ├── launch │ │ ├── add_car_to_world.launch │ │ ├── demo_gazebo_sc0.launch │ │ ├── demo_move_base_amcl.launch │ │ ├── multi_car.launch │ │ ├── rviz.launch │ │ ├── sc_gazebo.launch │ │ ├── spawn_A3_B3.launch │ │ ├── spawn_a_car.launch │ │ ├── spawn_test_single_car.launch │ │ └── test_car_with_empty_world.launch │ ├── map │ │ ├── magichouse.pgm │ │ ├── magichouse.yaml │ │ ├── playground.pgm │ │ ├── playground.yaml │ │ ├── willowgarage_sc.pgm │ │ └── willowgarage_sc.yaml │ ├── materials │ │ └── textures │ │ │ └── kinect.png │ ├── meshes │ │ ├── base_link.stl │ │ ├── hokuyo.dae │ │ ├── kinect.dae │ │ ├── mecanum_left_hands.stl │ │ └── mecanum_right_hands.stl │ ├── package.xml │ ├── rviz │ │ ├── navigation.rviz │ │ └── sc_model.rviz │ ├── urdf │ │ ├── base_car.urdf.xacro │ │ ├── base_drive.gazebo │ │ ├── mecanum.xacro │ │ ├── mecanum_base.xacro │ │ ├── reference.urdf │ │ ├── sc.plugins.xacro │ │ ├── sc0.urdf.xacro │ │ ├── sc_laser_camera.gazebo │ │ └── sc_mecanum.urdf.xacro │ └── worlds │ │ ├── magichouse.world │ │ ├── playground.world │ │ ├── real_world.world │ │ └── willowgarage.world │ └── sim_platform_pysdk │ ├── CMakeLists.txt │ ├── Makefile │ ├── README.md │ ├── launch │ ├── start_2AI.launch │ └── start_computer_and_your_code.launch │ ├── package.xml │ └── src │ ├── common │ ├── __init__.py │ └── obstacles.py │ ├── computer │ ├── AI.so │ ├── computer.py │ ├── computer_interface.so │ └── mymath.so │ ├── player │ ├── example.py │ ├── example.pyc │ ├── plan_points.py │ ├── plan_points.pyc │ ├── player.py │ ├── player_interface.so │ ├── test1move.py │ ├── test3moves.py │ ├── test_move_attack.py │ └── testattack.py │ └── setup.py ├── docs └── maze_easy.vsdx ├── img ├── 0.png ├── 1.png ├── 2.png ├── 3.png ├── 4.png ├── 5.png ├── 7.png ├── maze_easy.png ├── maze_easy_data.png ├── maze_easy_data_grid.png ├── maze_hard.png ├── perspective.mp4 ├── test.mp4 ├── test2.mp4 ├── test3.mp4 └── uav_move.mp4 ├── imu_processing.py ├── judge_uav_solve_maze.py ├── maze_map_data ├── get_index_from_pos.m ├── get_pos_from_map_index.m ├── maze_map.mat └── test.m ├── pub_target_circle.py ├── pub_target_line_x.py ├── pub_target_line_y.py ├── pub_target_line_z.py ├── pub_target_test.py ├── pub_two_drone.py ├── start_maze_ctrl.sh ├── start_maze_hard_ctrl.sh ├── start_maze_hard_hector_with_car.sh ├── start_maze_use_car.sh ├── start_maze_use_hector.sh ├── start_test_uav1.sh ├── start_test_uav1_with_downward_cam.sh └── test_car1_close_loop_control.py /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | /catkin_ws/build 3 | /catkin_ws/devel 4 | /catkin_ws/.vscode 5 | /catkin_ws/.catkin_tools 6 | /catkin_ws/logs 7 | .pyc 8 | *.cproject 9 | *.project 10 | *.settings -------------------------------------------------------------------------------- /.start_test_uav1_with_downward_cam.sh.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/.start_test_uav1_with_downward_cam.sh.swp -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | # Quick Start 2 | 3 | ## 环境准备 4 | ``` 5 | https://zhuanlan.zhihu.com/p/106032638 6 | ``` 7 | ## 无人机仿真之走迷宫 8 | ``` 9 | https://zhuanlan.zhihu.com/p/108374566 10 | ``` 11 | 12 | ### EASY 13 | 14 | ![maze_easy](https://github.com/smart-swarm/gazebo_drone_tutorials/blob/master/img/maze_easy.png) 15 | 16 | ### HARD 17 | 18 | ![maze_hard](https://github.com/smart-swarm/gazebo_drone_tutorials/blob/master/img/maze_hard.png) 19 | -------------------------------------------------------------------------------- /catkin_ws/battle.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sh python_kill.sh 4 | source $(pwd)/devel/setup.bash 5 | roslaunch sim_platform_pysdk start_computer_and_your_code.launch 6 | -------------------------------------------------------------------------------- /catkin_ws/kill.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | list='roslaunch 4 | rosmaster 5 | rosout 6 | rostopic 7 | gzclient 8 | gzserver 9 | robot_state_publisher 10 | message_to_tf/message_to_tf 11 | controller_manager/spawner 12 | topic_tools/relay 13 | hector_quad_simple_ctrl/hector_quad_simple_ctrl_twist_node 14 | hector_quad_simple_ctrl/hector_quad_simple_ctrl_pose_node 15 | judge_system 16 | spawn_model 17 | kinetic' 18 | 19 | for i in $list 20 | do 21 | ps -ef | grep $i | awk '{print $2}' | xargs -i kill -9 {} 22 | done 23 | 24 | ps -ef | grep -v sim_platform_pysdk | grep catkin_ws | awk '{print $2}' | xargs -i kill -9 {} 25 | 26 | sh python_kill.sh 27 | -------------------------------------------------------------------------------- /catkin_ws/load_bullet.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | num=$1 3 | list='uavA1 4 | uavA2 5 | uavA3 6 | carA1 7 | carA2 8 | carA3 9 | uavB1 10 | uavB2 11 | uavB3 12 | carB1 13 | carB2 14 | carB3' 15 | echo $num 16 | j=2 17 | for name in $list 18 | do 19 | echo $name 20 | for i in $(seq 1 $num); 21 | do 22 | echo $i 23 | echo "${name}_bullet_${i}" 24 | rosrun gazebo_ros spawn_model -file ~/.gazebo/models/my_bullet_control/model.sdf -sdf -x $i -y $(($j+500)) -z 10 -model "${name}_bullet_${i}" & 25 | done 26 | j=$(($j+2)) 27 | done 28 | 29 | -------------------------------------------------------------------------------- /catkin_ws/python_kill.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ps -ef | grep computer.py | grep python | awk '{print $2}' | xargs -i kill -9 {} 4 | 5 | ps -ef | grep player.py | grep python | awk '{print $2}' | xargs -i kill -9 {} 6 | -------------------------------------------------------------------------------- /catkin_ws/reset.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosnode kill /computer /player 3 | sh ./scripts/uav_disarm.sh 3 3 4 | rosservice call /reset_plat true 5 | rosnode kill /judge_system_node /judge_system_gui_node 6 | ps -ef | grep judge_system | awk '{print $2}' | xargs -i kill -9 {} 7 | sh ./scripts/reset_to_initial_pose.sh 8 | sh ./scripts/uav_arm.sh 3 3 9 | roslaunch judge_system judge_system_car6_uav6.launch path:=$(pwd) 10 | -------------------------------------------------------------------------------- /catkin_ws/scripts/load_bullet.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | num=$1 3 | list='uavA1 4 | uavA2 5 | uavA3 6 | carA1 7 | carA2 8 | carA3 9 | uavB1 10 | uavB2 11 | uavB3 12 | carB1 13 | carB2 14 | carB3' 15 | echo $num 16 | j=2 17 | for name in $list 18 | do 19 | echo $name 20 | for i in $(seq 1 $num); 21 | do 22 | echo $i 23 | echo "${name}_bullet_${i}" 24 | rosrun gazebo_ros spawn_model -file ~/.gazebo/models/my_bullet_control/model.sdf -sdf -x $i -y $(($j+500)) -z 10 -model "${name}_bullet_${i}" & 25 | done 26 | j=$(($j+2)) 27 | done 28 | 29 | -------------------------------------------------------------------------------- /catkin_ws/scripts/uav_arm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #echo -n "please enter the number of uav =:\n" 3 | #read num 4 | #echo -n "please enter the name of team =: A or B\n" 5 | #read team 6 | 7 | for i in $(seq 1 $1); 8 | do 9 | echo " uav$i armed."; 10 | rosservice call /uav$i/enable_motors true 11 | done 12 | exit 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /catkin_ws/scripts/uav_arm.sh.bak: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #echo -n "please enter the number of uav =:\n" 3 | #read num 4 | #echo -n "please enter the name of team =: A or B\n" 5 | #read team 6 | 7 | for i in $(seq 1 $1); 8 | do 9 | echo " A:uav$i armed."; 10 | rosservice call /A/uav$i/enable_motors true 11 | done 12 | 13 | for i in $(seq 1 $2); 14 | do 15 | echo " B:uav$i armed."; 16 | rosservice call /B/uav$i/enable_motors true 17 | done 18 | 19 | sleep 2 20 | 21 | for i in $(seq 1 $1); 22 | do 23 | echo " A:uav$i takeoff."; 24 | python ./src/hector_quad_simple_ctrl/scripts/takeoff_land_node.py /A/uav$i takeoff 10 & 25 | done 26 | 27 | for i in $(seq 1 $2); 28 | do 29 | echo " B:uav$i takeoff."; 30 | python ./src/hector_quad_simple_ctrl/scripts/takeoff_land_node.py /B/uav$i takeoff 10 & 31 | done 32 | 33 | exit 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /catkin_ws/scripts/uav_disarm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #echo -n "please enter the number of uav =:\n" 3 | #read num 4 | #echo -n "please enter the name of team =: A or B\n" 5 | #read team 6 | 7 | for i in $(seq 1 $1); 8 | do 9 | echo " A:uav$i armed."; 10 | rosservice call /A/uav$i/enable_motors false 11 | done 12 | 13 | for i in $(seq 1 $2); 14 | do 15 | echo " B:uav$i armed."; 16 | rosservice call /B/uav$i/enable_motors false 17 | done 18 | 19 | exit 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes ros-industrial/industrial_ci package. 2 | # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst 3 | sudo: required 4 | language: generic 5 | env: 6 | matrix: 7 | - ROS_DISTRO="kinetic" 8 | - ROS_DISTRO="melodic" 9 | install: 10 | - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .ci_config 11 | script: 12 | - source .ci_config/travis.sh 13 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/Makefile: -------------------------------------------------------------------------------- 1 | PREFIX ?= /usr/local 2 | 3 | CC = gcc 4 | AR = ar 5 | 6 | CFLAGS = -std=gnu99 -fPIC -Wall -Wno-unused-parameter -Wno-unused-function 7 | CFLAGS += -I. -O3 -fno-strict-overflow 8 | 9 | APRILTAG_SRCS := $(shell ls *.c common/*.c) 10 | APRILTAG_HEADERS := $(shell ls *.h common/*.h) 11 | APRILTAG_OBJS := $(APRILTAG_SRCS:%.c=%.o) 12 | TARGETS := libapriltag.a libapriltag.so 13 | 14 | .PHONY: all 15 | all: $(TARGETS) 16 | @$(MAKE) -C example all 17 | 18 | .PHONY: install 19 | install: libapriltag.so 20 | @chmod +x install.sh 21 | @./install.sh $(PREFIX)/lib libapriltag.so 22 | @./install.sh $(PREFIX)/include/apriltag $(APRILTAG_HEADERS) 23 | @ldconfig 24 | 25 | libapriltag.a: $(APRILTAG_OBJS) 26 | @echo " [$@]" 27 | @$(AR) -cq $@ $(APRILTAG_OBJS) 28 | 29 | libapriltag.so: $(APRILTAG_OBJS) 30 | @echo " [$@]" 31 | @$(CC) -fPIC -shared -o $@ $^ 32 | 33 | %.o: %.c 34 | @echo " $@" 35 | @$(CC) -o $@ -c $< $(CFLAGS) 36 | 37 | .PHONY: clean 38 | clean: 39 | @rm -rf *.o common/*.o $(TARGETS) 40 | @$(MAKE) -C example clean 41 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/apriltag.pc.in: -------------------------------------------------------------------------------- 1 | prefix= 2 | exec_prefix=${prefix} 3 | includedir=${prefix}/include 4 | libdir=${exec_prefix}/lib 5 | 6 | Name: apriltag 7 | Description: AprilTag detector library 8 | Version: 0.10.0 9 | Cflags: -I${includedir} 10 | Libs: -L${libdir} -lapriltag 11 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/example/.gitignore: -------------------------------------------------------------------------------- 1 | apriltag_demo 2 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/example/Makefile: -------------------------------------------------------------------------------- 1 | CC = gcc 2 | CXX = g++ 3 | 4 | CPPFLAGS = -I.. 5 | CFLAGS = -g -std=gnu99 -Wall -Wno-unused-parameter -Wno-unused-function -O3 6 | CXXFLAGS = -g -Wall -O3 7 | LDFLAGS = -lpthread -lm 8 | 9 | TARGETS := apriltag_demo opencv_demo 10 | 11 | .PHONY: all 12 | all: apriltag_demo opencv_demo 13 | 14 | apriltag_demo: apriltag_demo.o ../libapriltag.a 15 | @echo " [$@]" 16 | @$(CC) -o $@ $^ $(LDFLAGS) 17 | 18 | opencv_demo: opencv_demo.o ../libapriltag.a 19 | @echo " [$@]" 20 | @$(CXX) -o $@ $^ $(LDFLAGS) `pkg-config --libs opencv` 21 | 22 | %.o: %.c 23 | @echo " $@" 24 | @$(CC) -o $@ -c $< $(CFLAGS) $(CPPFLAGS) 25 | 26 | %.o: %.cc 27 | @echo " $@" 28 | @$(CXX) -o $@ -c $< $(CXXFLAGS) $(CPPFLAGS) 29 | 30 | .PHONY: clean 31 | clean: 32 | @rm -rf *.o $(TARGETS) 33 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/example/README: -------------------------------------------------------------------------------- 1 | These example programs are meant for distribution, and thus will not build in the april2 tree without modifications. 2 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh -e 2 | 3 | # Usage: install.sh TARGET [RELATIVE PATHS ...] 4 | # 5 | # e.g. ./install.sh /usr/local foo/file1 foo/file2 ... 6 | # This creates the files /usr/local/foo/file1 and /usr/local/foo/file2 7 | 8 | TARGETDIR=$1 9 | shift 10 | 11 | for src in "$@"; do 12 | dest=$TARGETDIR/$src 13 | mkdir -p $(dirname $dest) 14 | cp $src $dest 15 | echo $dest 16 | done 17 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | apriltag 5 | 3.1.1 6 | AprilTag detector library 7 | 8 | Max Krogius 9 | Wolfgang Merkt 10 | 11 | Edwin Olson 12 | Max Krogius 13 | 14 | BSD 15 | https://april.eecs.umich.edu/software/apriltag.html 16 | 17 | cmake 18 | 19 | 20 | cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag/python_build_flags.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import sysconfig 3 | import re 4 | import numpy as np 5 | conf = sysconfig.get_config_vars() 6 | 7 | print('CFLAGS', end=';') 8 | c_flags = [] 9 | # Grab compiler flags minus the compiler itself. 10 | c_flags.extend(conf.get('CC', '').split()[2:]) 11 | c_flags.extend(conf.get('CFLAGS', '').split()) 12 | c_flags.extend(conf.get('CCSHARED', '').split()) 13 | c_flags.append('-I{}'.format(conf.get('INCLUDEPY', ''))) 14 | c_flags.append('-I{}'.format(np.get_include())) 15 | c_flags.append('-Wno-strict-prototypes') 16 | c_flags = [x for x in c_flags if not x.startswith('-O')] 17 | print(' '.join(c_flags), end=';') 18 | 19 | 20 | print('LINKER', end=';') 21 | print(conf.get('BLDSHARED', '').split()[0], end=';') 22 | 23 | print('LDFLAGS', end=';') 24 | print(' '.join(conf.get('BLDSHARED', '').split()[1:]) + ' ' + conf.get('BLDLIBRARY', '') + ' ' + conf.get('LDFLAGS', ''), end=';') 25 | 26 | print('EXT_SUFFIX', end=';') 27 | ext_suffix = '.so' 28 | if 'EXT_SUFFIX' in conf: 29 | ext_suffix = conf['EXT_SUFFIX'] 30 | elif 'MULTIARCH' in conf: 31 | ext_suffix = '.' + conf['MULTIARCH'] + '.so' 32 | 33 | print(ext_suffix, end=';') 34 | 35 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package apriltag_ros 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 3.1.1 (2019-10-07) 6 | ------------------ 7 | * Add support for tagStandard41h12 and tagStandard52h13 (`#63 `_, `#59 `_). 8 | * Add gray image input support (`#58 `_). 9 | * Contributors: Alexander Reimann, Moritz Zimmermann, Samuel Bachmann, Wolfgang Merkt 10 | 11 | 3.1.0 (2019-05-25) 12 | ------------------ 13 | * Prepare for release (3.1) and fix catkin_lint errors 14 | * Upgrade to AprilTag 3, fix installation, and performance improvements (`#43 `_) 15 | Upgrade to AprilTag 3, fix installation, and performance improvements 16 | * Rename package to apriltag_ros 17 | - Relates to https://github.com/AprilRobotics/apriltag_ros/issues/45 18 | * Contributors: Wolfgang Merkt 19 | 20 | 1.0.0 (2018-05-14) 21 | ------------------ 22 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/config/settings.yaml: -------------------------------------------------------------------------------- 1 | # AprilTag 3 code parameters 2 | # Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector 3 | # apriltag/include/apriltag.h:struct apriltag_family 4 | tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 5 | tag_threads: 4 # default: 2 6 | tag_decimate: 1.0 # default: 1.0 7 | tag_blur: 0.0 # default: 0.0 8 | tag_refine_edges: 1 # default: 1 9 | tag_debug: 0 # default: 0 10 | # Other parameters 11 | publish_tf: true # default: false 12 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/launch/continuous_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/launch/single_image_client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/launch/single_image_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/msg/AprilTagDetection.msg: -------------------------------------------------------------------------------- 1 | # Tag ID(s). If a standalone tag, this is a vector of size 1. If a tag bundle, 2 | # this is a vector containing the IDs of each tag in the bundle. 3 | int32[] id 4 | 5 | # Tag size(s). If a standalone tag, this is a vector of size 1. If a tag bundle, 6 | # this is a vector containing the sizes of each tag in the bundle, in the same 7 | # order as the IDs above. 8 | float64[] size 9 | 10 | # Pose in the camera frame, obtained from homography transform. If a standalone 11 | # tag, the homography is from the four tag corners. If a tag bundle, the 12 | # homography is from at least the four corners of one member tag and at most the 13 | # four corners of all member tags. 14 | geometry_msgs/PoseWithCovarianceStamped pose -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/msg/AprilTagDetectionArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | AprilTagDetection[] detections 3 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Continuous AprilTag 3 detector 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/scripts/analyze_image: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # 3 | # Detect AprilTag in a single image. Usage: 4 | # 5 | # ./analyze_image.sh 6 | # 7 | 8 | roslaunch apriltag_ros single_image_client.launch image_load_path:="$1" image_save_path:="$2" 9 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/apriltag_ros/apriltag_ros/srv/AnalyzeSingleImage.srv: -------------------------------------------------------------------------------- 1 | # Service which takes in: 2 | # 3 | # full_path_to_image : full path to a .jpg image 4 | # 5 | # and returns: 6 | # 7 | # pose : the pose of the tag in the camera frame 8 | # tag_detection_image : an image with the detected tag's border highlighted and payload value printed 9 | 10 | string full_path_where_to_get_image 11 | string full_path_where_to_save_image 12 | sensor_msgs/CameraInfo camera_info 13 | --- 14 | apriltag_ros/AprilTagDetectionArray tag_detections -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/integrated_navigation/launch/integrated_navigation_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/integrated_navigation/scripts/tools.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/QRtags_imu_navigation/integrated_navigation/scripts/tools.pyc -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/.gitignore: -------------------------------------------------------------------------------- 1 | doc/html 2 | *.cproject 3 | *.project 4 | *.settings 5 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/README.md: -------------------------------------------------------------------------------- 1 | robot_localization 2 | ================== 3 | 4 | robot_localization is a package of nonlinear state estimation nodes. The package was developed by Charles River Analytics, Inc. 5 | 6 | Please see documentation here: http://docs.ros.org/melodic/api/robot_localization/html/index.html 7 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/.templates/full_globaltoc.html: -------------------------------------------------------------------------------- 1 |

{{ _('Table Of Contents') }}

2 | {{ toctree(includehidden=True) }} 3 | 4 |

{{ _('Further Documentation') }}

5 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ../CHANGELOG.rst -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/images/figure1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/images/figure1.png -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/images/rl_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/images/rl_small.png -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/manifest.yaml: -------------------------------------------------------------------------------- 1 | actions: [] 2 | authors: Tom Moore 3 | brief: '' 4 | bugtracker: '' 5 | depends: 6 | - catkin 7 | - eigen 8 | - diagnostic_updater 9 | - cmake_modules 10 | - tf2 11 | - nav_msgs 12 | - roscpp 13 | - rostest 14 | - tf2_ros 15 | - message_generation 16 | - message_filters 17 | - tf2_geometry_msgs 18 | - sensor_msgs 19 | - message_runtime 20 | - std_msgs 21 | - roslint 22 | - rosunit 23 | - diagnostic_msgs 24 | - geographic_msgs 25 | - xmlrpcpp 26 | - python-catkin-pkg 27 | - geometry_msgs 28 | - rosbag 29 | description: The robot_localization package provides nonlinear state estimation through 30 | sensor fusion of an abritrary number of sensors. 31 | license: BSD 32 | maintainers: Tom Moore 33 | msgs: [] 34 | package_type: package 35 | repo_url: '' 36 | srvs: 37 | - SetPose 38 | - SetDatum 39 | url: http://ros.org/wiki/robot_localization 40 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/robot_localization_ias13_revised.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/robot_localization_ias13_revised.pdf -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/doc/user_contributed_tutorials.rst: -------------------------------------------------------------------------------- 1 | User-Contributed Tutorials 2 | ########################## 3 | 4 | Here's a list of user-contributed tutorials for robot_localization! 5 | 6 | Tutorials 7 | ========= 8 | 9 | * `ros-sensor-fusion-tutorial `_ 10 | A comprehensive end-to-end tutorial for setting up robot_localization for sensor fusion, as well as running through the necessary concepts. (Includes a practical example of setting it up with ultrasonic beacons!) 11 | 12 | * `Kapernikov: The ROS robot_localization package `_ 13 | The documentation of the robot_localization package is quite clear once you know how it works. However, it lacks a hands-on tutorial to help you with your first steps. There are some great examples on how to set up the robot_localization package, but they require good working hardware. This tutorial tries to bridge the gap, using the turtlesim package as a virtual robot. 14 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/launch/ekf_template.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/launch/ukf_template.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ekf_localization_node as nodelet 7 | 8 | 9 | 10 | 11 | 14 | 15 | ukf_localization_node as nodelet 16 | 17 | 18 | 19 | 20 | 23 | 24 | navsat_transform_node as nodelet 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: doc 3 | - builder: doxygen 4 | output_dir: api 5 | file_patterns: '*.cpp *.h *.dox *.md' 6 | exclude_patterns: '*/test/*' 7 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/srv/FromLL.srv: -------------------------------------------------------------------------------- 1 | geographic_msgs/GeoPoint ll_point 2 | --- 3 | geometry_msgs/Point map_point 4 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/srv/GetState.srv: -------------------------------------------------------------------------------- 1 | time time_stamp 2 | string frame_id 3 | --- 4 | # State vector: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az 5 | float64[15] state 6 | 7 | # Covariance matrix in row-major order 8 | float64[225] covariance 9 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/srv/SetDatum.srv: -------------------------------------------------------------------------------- 1 | geographic_msgs/GeoPose geo_pose 2 | --- 3 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/srv/SetPose.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseWithCovarianceStamped pose 2 | --- 3 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/srv/ToLL.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point map_point 2 | --- 3 | geographic_msgs/GeoPoint ll_point 4 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/srv/ToggleFilterProcessing.srv: -------------------------------------------------------------------------------- 1 | bool on 2 | --- 3 | bool status 4 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/record_all_stats.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ./stat_recorder.sh test_ekf_localization_node_bag1.test $1/ekf1.txt 4 | ./stat_recorder.sh test_ekf_localization_node_bag2.test $1/ekf2.txt 5 | ./stat_recorder.sh test_ekf_localization_node_bag3.test $1/ekf3.txt 6 | 7 | ./stat_recorder.sh test_ukf_localization_node_bag1.test $1/ukf1.txt 8 | ./stat_recorder.sh test_ukf_localization_node_bag2.test $1/ukf2.txt 9 | ./stat_recorder.sh test_ukf_localization_node_bag3.test $1/ukf3.txt 10 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/stat_recorder.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rm $2 4 | echo "x = [" > $2 5 | 6 | i="0" 7 | 8 | while [ $i -lt 30 ] 9 | do 10 | rostest robot_localization $1 output_final_position:=true output_location:=$2 11 | i=$[$i+1] 12 | sleep 3 13 | done 14 | 15 | echo "]" >> $2 16 | echo "mean(x)" >> $2 17 | echo "sqrt(sum((4 * std(x)).^2))" >> $2 18 | 19 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test1.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test1.bag -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test2.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test2.bag -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test3.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test3.bag -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test_ekf.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test_robot_localization_estimator.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/QRtags_imu_navigation/robot_localization/test/test_ukf.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/bullet_control/cmd: -------------------------------------------------------------------------------- 1 | rosrun gazebo_ros spawn_model -file ~/.gazebo/models/my_bullet_control/model.sdf -sdf -x 1 -y 2 -z 3 -model test 2 | rostopic pub /bullet_AA1/vel_cmd geometry_msgs/Vector3 '{x: 1.1, y: 0.0, z: 5.0}' 3 | export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:~/catkin_ws/devel/lib 4 | -------------------------------------------------------------------------------- /catkin_ws/src/bullet_control/sdf/my_bullet_control/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_bullet_control 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/bullet_control/urdf/model.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 0 10 | 0 11 | 0 12 | 0 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /catkin_ws/src/bullet_control/world/bullet.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 15 | 16 | model://my_bullet_control 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/include/authorized_check.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class AuthorizedCheck { 12 | public: 13 | AuthorizedCheck() {} 14 | ~AuthorizedCheck() { 15 | close(_socket_fd); 16 | } 17 | 18 | int init(const std::string& ip_str, int32_t port); 19 | void gen_request(const std::string& content); 20 | int run_check(); 21 | 22 | private: 23 | void send_request(); 24 | void trans_response(); 25 | int do_check(); 26 | 27 | void split(std::string strtem, char a, std::vector& output_vec); 28 | 29 | std::string _ip_str; 30 | int32_t _port = 0; 31 | int32_t _socket_fd = 0; 32 | struct sockaddr_in _address; 33 | struct hostent* _server_ptr; 34 | std::stringstream _request; 35 | char _buffer[1024 * 1024] = {0}; 36 | }; 37 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/launch/hector_pose_ctrl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/launch/hector_twist_ctrl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/scripts/pub_multiple_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import sys 4 | from std_msgs.msg import Header 5 | from geometry_msgs.msg import Twist 6 | 7 | pose_vel_ctrl = Twist() 8 | 9 | def fly2goal(drone, X,Y,Z,Yaw): 10 | rospy.init_node('fly2goal', anonymous=True) 11 | pub_topic_name = drone+"/pose_cmd" 12 | pub_pose_cmd = rospy.Publisher(pub_topic_name, Twist, queue_size=100) 13 | i = 0 14 | rate = rospy.Rate(30) 15 | while not rospy.is_shutdown(): 16 | pose_vel_ctrl.linear.x = X 17 | pose_vel_ctrl.linear.y = Y 18 | pose_vel_ctrl.linear.z = Z 19 | pose_vel_ctrl.angular.x = 0.0 20 | pose_vel_ctrl.angular.y = 0.0 21 | pose_vel_ctrl.angular.z = Yaw 22 | pub_pose_cmd.publish(pose_vel_ctrl) 23 | rate.sleep() 24 | 25 | if __name__ == '__main__': 26 | 27 | drone = str(sys.argv[1]) 28 | X = float(sys.argv[2]) 29 | Y = float(sys.argv[3]) 30 | Z = float(sys.argv[4]) 31 | Yaw = float(sys.argv[5]) 32 | # drone=input() 33 | # cmd=input() 34 | fly2goal(drone, X,Y,Z,Yaw) -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/scripts/pub_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import sys 4 | from std_msgs.msg import Header 5 | from geometry_msgs.msg import Twist 6 | 7 | pose_vel_ctrl = Twist() 8 | 9 | def fly2goal(drone, X,Y,Z,Yaw): 10 | rospy.init_node('fly2goal', anonymous=True) 11 | pub_topic_name = drone+"/pose_cmd" 12 | pub_pose_cmd = rospy.Publisher(pub_topic_name, Twist, queue_size=100) 13 | i = 0 14 | rate = rospy.Rate(30) 15 | while not rospy.is_shutdown(): 16 | pose_vel_ctrl.linear.x = X; 17 | pose_vel_ctrl.linear.y = Y; 18 | pose_vel_ctrl.linear.z = Z; 19 | pose_vel_ctrl.angular.x = 0.0; 20 | pose_vel_ctrl.angular.y = 0.0; 21 | pose_vel_ctrl.angular.z = Yaw; 22 | pub_pose_cmd.publish(pose_vel_ctrl) 23 | rate.sleep() 24 | 25 | if __name__ == '__main__': 26 | 27 | drone = str(sys.argv[1]) 28 | X = float(sys.argv[2]) 29 | Y = float(sys.argv[3]) 30 | Z = float(sys.argv[4]) 31 | Yaw = float(sys.argv[5]) 32 | # drone=input() 33 | # cmd=input() 34 | fly2goal(drone, X,Y,Z,Yaw) -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/sh_scripts/pub_vel_uav1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rostopic pub -r 10 /uav1/velocity_cmd geometry_msgs/Twist '{linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.1}}' & PID0=$! 3 | wait 4 | kill PID0 5 | exit 6 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/sh_scripts/start_uav1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosservice call /uav1/enable_motors true 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/sh_scripts/stop_uav1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosservice call /uav1/enable_motors false 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/sh_scripts/test_cmd.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rostopic pub -r 10 /uav1/cmd_vel geometry_msgs/Twist '{linear: {x: 0, y: 0.0, z: 0.3}, angular: {x: 0.0,y: 0.0,z: 0.0}}' &PID0=$! 3 | wait 4 | kill PID0 5 | exit 6 | 7 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quad_simple_ctrl/sh_scripts/uav1_takeoff: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ./start_uav1.sh 3 | python /home/zhenglong/apriltag_ws/src/hector_quad_simple_ctrl/scripts/takeoff_land_node.py uav1 takeoff 4 | 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_gazebo_plugins/.gitignore: -------------------------------------------------------------------------------- 1 | *.user -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_gazebo_plugins/cfg/GNSS.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "hector_gazebo_plugins" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("STATUS_FIX", bool_t, 1, "unaugmented fix", True) 9 | gen.add("STATUS_SBAS_FIX", bool_t, 1, "fix with satellite-based augmentation", False) 10 | gen.add("STATUS_GBAS_FIX", bool_t, 1, "with ground-based augmentation", False) 11 | 12 | gen.add("SERVICE_GPS", bool_t, 1, "GPS service", True) 13 | gen.add("SERVICE_GLONASS", bool_t, 1, "GLONASS service", True) 14 | gen.add("SERVICE_COMPASS", bool_t, 1, "COMPASS service", True) 15 | gen.add("SERVICE_GALILEO", bool_t, 1, "GALILEO service", True) 16 | 17 | exit(gen.generate(PACKAGE, "hector_gazebo_plugins", "GNSS")) 18 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_gazebo_plugins/cfg/SensorModel.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "hector_gazebo_plugins" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("gaussian_noise", double_t, 1, "Standard deviation of the additive white Gaussian noise", 0.0, 0.0, 10.0) 9 | gen.add("offset", double_t, 1, "Zero-offset of the published sensor signal", 0.0, -10.0, 10.0) 10 | gen.add("drift", double_t, 1, "Standard deviation of the sensor drift", 0.0, 0.0, 10.0) 11 | gen.add("drift_frequency", double_t, 1, "Reciprocal of the time constant of the first-order drift model in Hz", 0.0, 0.0, 1.0) 12 | gen.add("scale_error", double_t, 1, "Scale error", 1.0, 0.0, 2.0) 13 | 14 | exit(gen.generate(PACKAGE, "hector_gazebo_plugins", "SensorModel")) 15 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_gazebo_plugins/srv/SetBias.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Vector3 bias 2 | --- 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: kinetic-devel} 2 | - git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin} 3 | - git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: kinetic-devel} 4 | - git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: kinetic-devel} 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | * moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo 14 | * Contributors: Johannes Meyer 15 | 16 | 0.3.2 (2014-03-30) 17 | ------------------ 18 | * added packages hector_quadrotor_controller and hector_quadrotor_controller_gazebo to meta-package 19 | * Contributors: Johannes Meyer 20 | 21 | 0.3.1 (2013-12-26) 22 | ------------------ 23 | 24 | 0.3.0 (2013-09-11) 25 | ------------------ 26 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 27 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor 3 | 0.3.5 4 | hector_quadrotor contains packages related to modeling, control and simulation of quadrotor UAV systems 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | Stefan Kohlbrecher 14 | 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | 22 | hector_quadrotor_controllers 23 | hector_quadrotor_description 24 | hector_quadrotor_model 25 | hector_quadrotor_teleop 26 | hector_uav_msgs 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_actions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_actions) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | actionlib 6 | geometry_msgs 7 | hector_quadrotor_interface 8 | hector_uav_msgs 9 | roscpp 10 | tf2 11 | tf2_geometry_msgs 12 | ) 13 | include_directories(include ${catkin_INCLUDE_DIRS}) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES 18 | CATKIN_DEPENDS roscpp 19 | DEPENDS 20 | ) 21 | 22 | add_executable(landing_action 23 | src/landing_action.cpp 24 | ) 25 | target_link_libraries(landing_action ${catkin_LIBRARIES}) 26 | add_dependencies(landing_action ${catkin_EXPORTED_TARGETS}) 27 | 28 | add_executable(pose_action 29 | src/pose_action.cpp 30 | ) 31 | target_link_libraries(pose_action ${catkin_LIBRARIES}) 32 | add_dependencies(pose_action ${catkin_EXPORTED_TARGETS}) 33 | 34 | add_executable(takeoff_action 35 | src/takeoff_action.cpp 36 | ) 37 | target_link_libraries(takeoff_action ${catkin_LIBRARIES}) 38 | add_dependencies(takeoff_action ${catkin_EXPORTED_TARGETS}) 39 | 40 | install(TARGETS landing_action pose_action takeoff_action 41 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 47 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_actions/launch/actions.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_actions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_quadrotor_actions 4 | 0.0.0 5 | The hector_quadrotor_actions package provides action server implementations for some complex behaviors of the quadrotor. 6 | Johannes Meyer 7 | 8 | BSD 9 | 10 | Paul Bovbel 11 | 12 | catkin 13 | 14 | actionlib 15 | geometry_msgs 16 | hector_quadrotor_interface 17 | hector_uav_msgs 18 | roscpp 19 | tf2 20 | tf2_geometry_msgs 21 | 22 | actionlib 23 | geometry_msgs 24 | hector_quadrotor_interface 25 | hector_uav_msgs 26 | roscpp 27 | tf2 28 | tf2_geometry_msgs 29 | 30 | 31 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_controller_gazebo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-12-26) 18 | ------------------ 19 | * New controller implementation using ros_control 20 | * Contributors: Johannes Meyer 21 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_controller_gazebo) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | gazebo_ros_control 6 | hector_quadrotor_interface 7 | ) 8 | include_directories(include ${catkin_INCLUDE_DIRS}) 9 | 10 | find_package(gazebo REQUIRED) 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 12 | link_directories(${GAZEBO_LIBRARY_DIRS}) 13 | include_directories(${GAZEBO_INCLUDE_DIRS}) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES hector_quadrotor_controller_gazebo 18 | CATKIN_DEPENDS gazebo_ros_control hector_quadrotor_interface 19 | ) 20 | 21 | add_library(hector_quadrotor_controller_gazebo 22 | src/quadrotor_hardware_gazebo.cpp 23 | ) 24 | target_link_libraries(hector_quadrotor_controller_gazebo 25 | ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} 26 | ) 27 | 28 | install(TARGETS hector_quadrotor_controller_gazebo 29 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 30 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 32 | ) 33 | 34 | install(DIRECTORY include/${PROJECT_NAME}/ 35 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 36 | ) 37 | 38 | install(FILES 39 | quadrotor_controller_gazebo.xml 40 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 41 | ) 42 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_quadrotor_controller_gazebo 4 | 0.3.5 5 | The hector_quadrotor_controller_gazebo package implements the ros_control RobotHWSim interface for the quadrotor controller in package hector_quadrotor_controller. 6 | Johannes Meyer 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/hector_quadrotor_controller_gazebo 11 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 12 | 13 | Johannes Meyer 14 | 15 | catkin 16 | gazebo_ros_control 17 | hector_quadrotor_interface 18 | gazebo_ros_control 19 | hector_quadrotor_interface 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This class represents the quadrotor hardware in Gazebo for the gazebo_ros_control plugin. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_controllers) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | control_toolbox 6 | controller_interface 7 | geometry_msgs 8 | hector_quadrotor_interface 9 | hector_uav_msgs 10 | roscpp 11 | tf 12 | tf2_geometry_msgs 13 | visualization_msgs 14 | ) 15 | include_directories(${catkin_INCLUDE_DIRS}) 16 | 17 | catkin_package( 18 | LIBRARIES hector_quadrotor_controllers 19 | CATKIN_DEPENDS roscpp 20 | DEPENDS 21 | ) 22 | 23 | add_library(hector_quadrotor_controllers 24 | src/attitude_controller.cpp 25 | src/velocity_controller.cpp 26 | src/position_controller.cpp 27 | ) 28 | target_link_libraries(hector_quadrotor_controllers ${catkin_LIBRARIES}) 29 | add_dependencies(hector_quadrotor_controllers ${catkin_EXPORTED_TARGETS}) 30 | 31 | install(TARGETS hector_quadrotor_controllers 32 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 35 | ) 36 | 37 | install(DIRECTORY launch params 38 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 39 | ) 40 | 41 | install(FILES 42 | plugin.xml 43 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 44 | ) 45 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controllers/launch/controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controllers/params/params.yaml: -------------------------------------------------------------------------------- 1 | connection_timeout: 10.0 2 | action_timeout: 30.0 3 | state_timeout: 0.5 4 | command_timeout: 0.5 5 | 6 | dist_tolerance: 0.1 7 | yaw_tolerance: 0.35 8 | time_in_tolerance: 1.0 9 | 10 | landing_height: 0.1 11 | takeoff_height: 0.1 12 | 13 | estop_deceleration: 1.0 14 | 15 | wrench_limits: 16 | force: # N 17 | z: 18 | min: 0.0 19 | max: 30.0 20 | torque: # Nm 21 | x: 22 | max: 10.0 23 | y: 24 | max: 10.0 25 | z: 26 | max: 1.0 27 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_controllers/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | TODO 7 | 8 | 9 | 11 | 12 | TODO 13 | 14 | 15 | 17 | 18 | TODO 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_demo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_demo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-12-26) 18 | ------------------ 19 | * updated demo launch files 20 | * updated rviz configs 21 | * Contributors: Johannes Meyer 22 | 23 | 0.3.0 (2013-09-11) 24 | ------------------ 25 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 26 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_demo/launch/indoor_slam_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_demo/launch/outdoor_flight_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_demo 3 | 0.3.5 4 | hector_quadrotor_demo contains various launch files and needed dependencies for demonstration of the hector_quadrotor stack (indoor/outdoor flight in gazebo etc.) 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor_demo 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Stefan Kohlbrecher 13 | 14 | 15 | catkin 16 | 17 | 18 | 19 | 20 | hector_quadrotor_gazebo 21 | hector_gazebo_worlds 22 | hector_mapping 23 | hector_geotiff 24 | hector_trajectory_server 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | 14 | 0.3.2 (2014-03-30) 15 | ------------------ 16 | 17 | 0.3.1 (2013-12-26) 18 | ------------------ 19 | 20 | 0.3.0 (2013-09-11) 21 | ------------------ 22 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 23 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/launch/xacrodisplay_quadrotor_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/blender/quadrotor_base.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/blender/quadrotor_base.blend -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/hector_quadrotor/hector_quadrotor_description/meshes/quadrotor/quadrotor_base.stl -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_quadrotor_description 3 | 0.3.5 4 | hector_quadrotor_description provides an URDF model of a quadrotor UAV. 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_quadrotor_description 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | Stefan Kohlbrecher 14 | 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | hector_sensors_description 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/blue/quadrotor_blue.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/blue/quadrotor_blue.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/blue/quadrotor_with_cam_blue.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/blue/quadrotor_with_cam_blue.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_downward_cam.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_hokuyo_utm30lx.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_asus_with_hokuyo_utm30lx.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_cam.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/quadrotor_with_kinect.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/red/quadrotor_red.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/red/quadrotor_red.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/red/quadrotor_with_cam_red.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_description/urdf/red/quadrotor_with_cam_red.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/quadrotor_empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/quadrotor_empty_world_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/sim_100_drones_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/sim_40_drones_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/sim_ten_drones_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_one_quadrotors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_one_quadrotors_for_maze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_one_quadrotors_for_maze_hard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/launch/spawn_quadrotor_with_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/box.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | Gazebo/Blue 23 | 24 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_aerodynamics.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | true 10 | 0.0 11 | base_link 12 | $(arg base_link_frame) 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_controller.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.05 7 | hector_quadrotor_controller_gazebo/QuadrotorHardwareSim 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_plugins.gazebo.xacro.in: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | @MODEL_PLUGINS_URDF@ 15 | 16 | 17 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_gazebo/urdf/quadrotor_propulsion.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | true 10 | 0.0 11 | base_link 12 | $(arg base_link_frame) 13 | 100.0 14 | 0.01 15 | 0.01 16 | 0.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_interface) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface tf2 tf2_geometry_msgs urdf) 5 | include_directories(include ${catkin_INCLUDE_DIRS}) 6 | 7 | find_package(Boost REQUIRED COMPONENTS system) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS include 11 | LIBRARIES hector_quadrotor_interface 12 | CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface 13 | DEPENDS 14 | ) 15 | 16 | add_library(hector_quadrotor_interface src/quadrotor_interface.cpp src/helpers.cpp) 17 | target_link_libraries(hector_quadrotor_interface ${catkin_LIBRARIES}) 18 | add_dependencies(hector_quadrotor_interface hector_uav_msgs_generate_messages_cpp) 19 | 20 | install(TARGETS hector_quadrotor_interface 21 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 22 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 23 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 24 | ) 25 | 26 | install(DIRECTORY include/${PROJECT_NAME}/ 27 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 28 | FILES_MATCHING PATTERN "*.h" 29 | ) 30 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/.gitignore: -------------------------------------------------------------------------------- 1 | matlab/codegen/ 2 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/matlab/compile_all.m: -------------------------------------------------------------------------------- 1 | rtw_config = coder.config('LIB'); 2 | rtw_config.TargetLang = 'C'; 3 | rtw_config.CodeReplacementLibrary = 'C89/C90 (ANSI)'; 4 | rtw_config.CCompilerOptimization = 'On'; 5 | 6 | codegen -config rtw_config -v quadrotorPropulsion motorspeed -o quadrotorPropulsion 7 | codegen -config rtw_config -v quadrotorDrag -o quadrotorDrag 8 | 9 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/matlab/dummy.c: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/matlab/parameter_QK_propulsion.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/hector_quadrotor/hector_quadrotor_model/matlab/parameter_QK_propulsion.m -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/matlab/quadrotorDrag.m: -------------------------------------------------------------------------------- 1 | function y = quadrotorDrag(uin, parameter, dt) 2 | %#eml 3 | 4 | assert(isa(uin,'double')); 5 | assert(all(size(uin) == [6 1])); 6 | 7 | assert(isa(parameter,'struct')); 8 | assert(isa(parameter.C_wxy,'double')); 9 | assert(isa(parameter.C_wz,'double')); 10 | 11 | assert(isa(parameter.C_mxy,'double')); 12 | assert(isa(parameter.C_mz,'double')); 13 | 14 | 15 | assert(isa(dt,'double')); 16 | assert(all(size(dt) == 1)); 17 | 18 | eml.cstructname(parameter,'DragParameters'); 19 | 20 | % initialize vectors 21 | y = zeros(6,1); 22 | 23 | % Input variables 24 | u = uin(1); 25 | v = uin(2); 26 | w = uin(3); 27 | p = uin(4); 28 | q = uin(5); 29 | r = uin(6); 30 | 31 | % Constants 32 | C_wxy = parameter.C_wxy; 33 | C_wz = parameter.C_wz; 34 | C_mxy = parameter.C_mxy; 35 | C_mz = parameter.C_mz; 36 | 37 | % temporarily used vector 38 | absoluteVelocity = sqrt(u^2 + v^2 + w^2); 39 | absoluteAngularVelocity = sqrt(p^2 + q^2 + r^2); 40 | 41 | % system outputs 42 | % calculate drag force 43 | y(1) = C_wxy* absoluteVelocity*u; 44 | y(2) = C_wxy* absoluteVelocity*v; 45 | y(3) = C_wz * absoluteVelocity*w; 46 | 47 | % calculate draq torque 48 | y(4) = C_mxy* absoluteAngularVelocity*p; 49 | y(5) = C_mxy* absoluteAngularVelocity*q; 50 | y(6) = C_mz * absoluteAngularVelocity*r; 51 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/param/quadrotor_aerodynamics.yaml: -------------------------------------------------------------------------------- 1 | quadrotor_aerodynamics: 2 | C_wxy: 0.12 3 | C_wz: 0.1 4 | C_mxy: 0.074156208000000 5 | C_mz: 0.050643264000000 -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1045.yaml: -------------------------------------------------------------------------------- 1 | quadrotor_propulsion: 2 | Psi: 0.007242179827506 3 | J_M: 2.573048063300000e-5 4 | R_A: 0.201084219222241 5 | k_t: 0.015336864714397 6 | k_m: -7.011631909766668e-5 7 | alpha_m: 0.104863758313889 8 | beta_m: 0.549262344777900 9 | 10 | #CT2s: -1.3077e-2 11 | CT2s: 0.0 12 | CT1s: -2.5224e-4 13 | CT0s: 1.538190483976698e-5 14 | 15 | l_m: 0.275 16 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_model/param/robbe_2827-34_epp1245.yaml: -------------------------------------------------------------------------------- 1 | quadrotor_propulsion: 2 | Psi: 0.007242179827506 3 | J_M: 4.142069415e-05 4 | R_A: 0.201084219222241 5 | k_t: 0.01732804081 6 | 7 | #CT2s: -2.3491553043010e-2 8 | CT2s: 0.0 9 | CT1s: -4.531245193522030e-04 10 | CT0s: 2.744543453e-05 11 | 12 | l_m: 0.275 13 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_pose_estimation/hector_quadrotor_pose_estimation_nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This nodelet implements a pose estimation filter which uses hector_uav_msgs/Altimeter messages as input for barometric height. 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_pose_estimation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_quadrotor_pose_estimation 4 | 0.3.5 5 | 6 | hector_quadrotor_pose_estimation provides a hector_pose_estimation node and nodelet specialized for hector_quadrotor. 7 | 8 | Johannes Meyer 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/hector_quadrotor_pose_estimation 13 | 14 | Johannes Meyer 15 | 16 | 17 | catkin 18 | 19 | 20 | hector_pose_estimation 21 | hector_uav_msgs 22 | 23 | 24 | hector_pose_estimation 25 | hector_uav_msgs 26 | nodelet 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_pose_estimation/params/simulation.yaml: -------------------------------------------------------------------------------- 1 | reference_altitude: 0.0 2 | reference_heading: 0.0 3 | reference_latitude: 49.860246 4 | reference_longitude: 8.687077 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_teleop/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_quadrotor_teleop 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | * Add optional parameter to set the joystick device 11 | * Contributors: whoenig 12 | 13 | 0.3.3 (2014-09-01) 14 | ------------------ 15 | * added run dependency to joy package (joystick drivers) 16 | * Contributors: Johannes Meyer 17 | 18 | 0.3.2 (2014-03-30) 19 | ------------------ 20 | 21 | 0.3.1 (2013-12-26) 22 | ------------------ 23 | * added slow button (btn 6 on Logitech Gamepad) 24 | * Contributors: Johannes Meyer 25 | 26 | 0.3.0 (2013-09-11) 27 | ------------------ 28 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 29 | * decreased z_velocity_max to 2.0 m/s in logitech_gamepad.launch 30 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_teleop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_quadrotor_teleop) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | actionlib 7 | roscpp 8 | sensor_msgs 9 | geometry_msgs 10 | hector_quadrotor_interface 11 | tf2_geometry_msgs 12 | visualization_msgs) 13 | include_directories(include ${catkin_INCLUDE_DIRS}) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS 17 | LIBRARIES 18 | CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs hector_uav_msgs 19 | DEPENDS 20 | ) 21 | 22 | add_executable(quadrotor_teleop src/quadrotor_teleop.cpp) 23 | target_link_libraries(quadrotor_teleop ${catkin_LIBRARIES}) 24 | add_dependencies(quadrotor_teleop ${catkin_EXPORTED_TARGETS}) 25 | 26 | install(TARGETS quadrotor_teleop 27 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 28 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 29 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 30 | ) 31 | 32 | install(DIRECTORY launch 33 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 34 | ) 35 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_teleop/launch/logitech_gamepad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | control_mode: $(arg control_mode) 18 | x_axis: 4 19 | y_axis: 3 20 | z_axis: 2 21 | thrust_axis: 2 22 | yaw_axis: 1 23 | 24 | slow_button: 6 25 | go_button: 4 26 | stop_button: 2 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_teleop/launch/sony_dualshock3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | control_mode: $(arg control_mode) 18 | x_axis: 4 19 | y_axis: 3 20 | z_axis: 2 21 | thrust_axis: 2 22 | yaw_axis: 1 23 | 24 | slow_button: 12 25 | go_button: 13 26 | stop_button: 15 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_quadrotor_teleop/launch/xbox_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | control_mode: $(arg control_mode) 18 | x_axis: 5 19 | y_axis: 4 20 | z_axis: 2 21 | thrust_axis: -3 22 | yaw_axis: 1 23 | 24 | slow_button: 4 25 | go_button: 6 26 | stop_button: 2 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hector_uav_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2015-03-28) 6 | ------------------ 7 | 8 | 0.3.4 (2015-02-22) 9 | ------------------ 10 | 11 | 0.3.3 (2014-09-01) 12 | ------------------ 13 | * updated value type in RawImu message to int16 14 | * added new ControllerState status constant FLYING 15 | * added CLIMBRATE and TURNRATE constants to ControllerState message 16 | * Contributors: Johannes Meyer 17 | 18 | 0.3.2 (2014-03-30) 19 | ------------------ 20 | 21 | 0.3.1 (2013-12-26) 22 | ------------------ 23 | 24 | 0.3.0 (2013-09-11) 25 | ------------------ 26 | * Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack 27 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_uav_msgs) 3 | 4 | find_package(catkin REQUIRED message_generation std_msgs geometry_msgs actionlib_msgs) 5 | 6 | add_message_files( 7 | FILES 8 | Altimeter.msg 9 | AttitudeCommand.msg 10 | Compass.msg 11 | ControllerState.msg 12 | HeadingCommand.msg 13 | HeightCommand.msg 14 | MotorCommand.msg 15 | MotorPWM.msg 16 | MotorStatus.msg 17 | PositionXYCommand.msg 18 | RawImu.msg 19 | RawMagnetic.msg 20 | RawRC.msg 21 | RC.msg 22 | RuddersCommand.msg 23 | ServoCommand.msg 24 | Supply.msg 25 | ThrustCommand.msg 26 | VelocityXYCommand.msg 27 | VelocityZCommand.msg 28 | YawrateCommand.msg 29 | ) 30 | 31 | add_service_files( 32 | FILES 33 | EnableMotors.srv 34 | ) 35 | 36 | add_action_files( 37 | DIRECTORY 38 | action 39 | FILES 40 | Pose.action 41 | Landing.action 42 | Takeoff.action 43 | ) 44 | 45 | generate_messages( 46 | DEPENDENCIES 47 | actionlib_msgs 48 | std_msgs 49 | geometry_msgs 50 | ) 51 | 52 | catkin_package( 53 | INCLUDE_DIRS include 54 | CATKIN_DEPENDS message_runtime std_msgs 55 | ) 56 | 57 | install(DIRECTORY include/${PROJECT_NAME}/ 58 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 59 | ) 60 | 61 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/action/Landing.action: -------------------------------------------------------------------------------- 1 | # Landing pose, pose.z is ignored. 2 | # If no stamp is provided, landing_zone is assumed to be empty and 3 | # robot will land directly below 4 | geometry_msgs/PoseStamped landing_zone 5 | --- 6 | --- 7 | geometry_msgs/PoseStamped current_pose 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/action/Pose.action: -------------------------------------------------------------------------------- 1 | # Pose command 2 | geometry_msgs/PoseStamped target_pose 3 | --- 4 | --- 5 | geometry_msgs/PoseStamped current_pose 6 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/action/Takeoff.action: -------------------------------------------------------------------------------- 1 | # Takeoff action, no input required 2 | --- 3 | --- 4 | geometry_msgs/PoseStamped current_pose 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/Altimeter.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 altitude 3 | float32 pressure 4 | float32 qnh 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/AttitudeCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 roll 3 | float32 pitch 4 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/Compass.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 magnetic_heading 3 | float32 declination 4 | 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/ControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 source 3 | 4 | uint8 mode 5 | uint8 MOTORS = 1 6 | uint8 ATTITUDE = 2 7 | uint8 VELOCITY = 4 8 | uint8 POSITION = 8 9 | uint8 TURNRATE = 16 10 | uint8 HEADING = 32 11 | uint8 CLIMBRATE = 64 12 | uint8 HEIGHT = 128 13 | 14 | uint8 state 15 | uint8 MOTORS_RUNNING = 1 16 | uint8 FLYING = 2 17 | uint8 AIRBORNE = 4 18 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/HeadingCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 heading 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/HeightCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 height 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/MotorCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32[] force 3 | float32[] torque 4 | float32[] frequency 5 | float32[] voltage 6 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/MotorPWM.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8[] pwm 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/MotorStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | bool on 3 | bool running 4 | float32[] voltage 5 | float32[] frequency 6 | float32[] current 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/PositionXYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/RC.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint8 ROLL = 1 4 | uint8 PITCH = 2 5 | uint8 YAW = 3 6 | uint8 STEER = 4 7 | uint8 HEIGHT = 5 8 | uint8 THRUST = 6 9 | uint8 BRAKE = 7 10 | 11 | uint8 status 12 | bool valid 13 | 14 | float32[] axis 15 | uint8[] axis_function 16 | 17 | int8[] swit 18 | uint8[] swit_function 19 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/RawImu.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int16[3] angular_velocity 3 | int16[3] linear_acceleration 4 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/RawMagnetic.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[3] channel 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/RawRC.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 status 3 | uint16[] channel 4 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/RuddersCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 aileron 3 | float32 elevator 4 | float32 rudder 5 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/ServoCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16[] value 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/Supply.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32[] voltage 3 | float32[] current 4 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/ThrustCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 thrust 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/VelocityXYCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/VelocityZCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 z 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/msg/YawrateCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 turnrate 3 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | hector_uav_msgs 3 | 0.3.5 4 | hector_uav_msgs is a message package that contains messages for UAV controller inputs and outputs and some sensor readings not covered by sensor_msgs. 5 | Johannes Meyer 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/hector_uav_msgs 10 | https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues 11 | 12 | Johannes Meyer 13 | 14 | 15 | catkin 16 | 17 | 18 | actionlib_msgs 19 | geometry_msgs 20 | message_generation 21 | std_msgs 22 | 23 | 24 | actionlib_msgs 25 | geometry_msgs 26 | message_runtime 27 | std_msgs 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/hector_uav_msgs/srv/EnableMotors.srv: -------------------------------------------------------------------------------- 1 | bool enable # enable or disable motors 2 | --- 3 | bool success # whether enabling or disabling was successful -------------------------------------------------------------------------------- /catkin_ws/src/hector_quadrotor/tutorials.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: kinetic-devel} 2 | - git: {local-name: hector_slam, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_slam.git', version: catkin} 3 | - git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin} 4 | - git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: kinetic-devel} 5 | - git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: kinetic-devel} 6 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package innok_heros_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.3 (2016-03-10) 6 | ------------------ 7 | * added missing run_depend 8 | * added joint_state_publisher to get correct tf when launching RViz 9 | * Contributors: Sabrina Heerklotz 10 | 11 | 1.0.2 (2015-12-11) 12 | ------------------ 13 | * Fixed CMakeLists.txt 14 | * Contributors: Sabrina Heerklotz 15 | 16 | 1.0.1 (2015-10-20) 17 | ------------------ 18 | * fixed dependencies 19 | * Contributors: Sabrina Heerklotz 20 | 21 | 1.0.0 (2015-10-15) 22 | ------------------ 23 | * initial commit 24 | * Contributors: Sabrina Heerklotz, Jonathan Hechtbauer 25 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(innok_heros_description) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | robot_state_publisher 6 | roslaunch 7 | urdf 8 | xacro 9 | ) 10 | catkin_package() 11 | roslaunch_add_file_check(launch) 12 | 13 | include_directories( 14 | ${catkin_INCLUDE_DIRS} 15 | ) 16 | 17 | 18 | install( 19 | DIRECTORY launch urdf meshes 20 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 21 | ) 22 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/README.md: -------------------------------------------------------------------------------- 1 | # innok_heros_description 2 | ROS Package with URDF files and launch files for Innok Heros 3 | 4 | for more information see http://wiki.ros.org/innok_heros_description 5 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/launch/innok_heros_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/base_link.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/battery_box.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/battery_box.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/beam.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/beam.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/caster_bot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/caster_bot.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/caster_top.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/caster_top.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/caster_wheel.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/caster_wheel.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/drive_box.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/drive_box.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/mounting_plate_3w.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/mounting_plate_3w.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/mounting_plate_4w.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/mounting_plate_4w.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/swing_axle_bot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/swing_axle_bot.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/swing_axle_top.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/swing_axle_top.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/wheel.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/wheel.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/wheel_tractor.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/wheel_tractor.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/meshes/wheel_tractor_large.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_description/meshes/wheel_tractor_large.STL -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | innok_heros_description 4 | 1.0.3 5 | Innok Heros URDF description and RVIZ launch file 6 | 7 | Sabrina Heerklotz 8 | Jonathan Hechtbauer 9 | 10 | http://wiki.ros.org/Robots/Innok-Heros 11 | 12 | BSD 13 | 14 | 15 | catkin 16 | roslaunch 17 | robot_state_publisher 18 | joint_state_publisher 19 | urdf 20 | xacro 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/urdf/ROS/heros_diff_drive.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | true 9 | 100 10 | joint_base_wheel_rear_left 11 | joint_base_wheel_rear_right 12 | ${wheel_seperation} 13 | ${2*wheel_radius} 14 | 130 15 | cmd_vel 16 | odom 17 | base_link 18 | false 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/urdf/ROS/heros_joint_pub.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | $(arg car_ns)/$(arg car_name) 8 | ${joints} 9 | 100.0 10 | true 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_description/urdf/ROS/heros_skid_drive.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 100.0 8 | /$(arg car_ns)/$(arg car_name) 9 | joint_base_wheel_front_left 10 | joint_base_wheel_front_right 11 | joint_base_wheel_rear_left 12 | joint_base_wheel_rear_right 13 | ${2*0.77} 14 | ${2*wheel_radius} 15 | base_link 16 | 130 17 | cmd_vel 18 | odom 19 | odom 20 | 1 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package innok_heros_gazebo 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.4 (2016-03-03) 6 | ------------------ 7 | * removed roslaunch_add_file_check 8 | * added robot_state_publisher to get correct tf tree 9 | * Contributors: Sabrina Heerklotz 10 | 11 | 1.0.3 (2016-02-11) 12 | ------------------ 13 | * removed run dependencies from Gazebo in package.xml 14 | * Contributors: Sabrina Heerklotz 15 | 16 | 1.0.2 (2015-12-11) 17 | ------------------ 18 | * Fixed CMakeLists.txt 19 | * Contributors: Sabrina Heerklotz 20 | 21 | 1.0.1 (2015-10-20) 22 | ------------------ 23 | * fixed dependencies 24 | * Contributors: Sabrina Heerklotz 25 | 26 | 1.0.0 (2015-10-15) 27 | ------------------ 28 | * initial commit 29 | * Contributors: Sabrina Heerklotz, Jonathan Hechtbauer 30 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(innok_heros_gazebo) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS roslaunch 6 | ) 7 | catkin_package() 8 | 9 | #roslaunch_add_file_check(launch) 10 | 11 | include_directories( 12 | ${catkin_INCLUDE_DIRS} 13 | ) 14 | 15 | install( 16 | DIRECTORY launch 17 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 18 | ) 19 | 20 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/README.md: -------------------------------------------------------------------------------- 1 | # innok_heros_gazebo 2 | ROS Package with launch files to simulate Innok Heros in Gazebo 3 | 4 | For more information see http://wiki.ros.org/innok_heros_gazebo 5 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/building_editor_models/lmx2Y2B/19052514CenterCylin/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19052514CenterCylin 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/building_editor_models/lmx2Y2B/19052514EdgeWall/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19052514EdgeWall 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/building_editor_models/lmx2Y2B/19052514EdgeWall_2m/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19052514EdgeWall_2m 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/building_editor_models/lmx2Y2B/head_texture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/building_editor_models/lmx2Y2B/head_texture.png -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/A/cars_A2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/A/uavs_A1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/B/cars_B2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/B/cars_B3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/B/uavs_B3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/cars_A_test_2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/cars_B_test_2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_cars10_uavs20.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_cars4_uavs6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_cars6_uavs6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_hard_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_maze_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_uav_B5_A8.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_world_1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_world_2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_world_3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_world_60x40.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/load_world_apriltag_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/launch/spawn_innok_vehicle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | innok_heros_gazebo 4 | 1.0.4 5 | Innok Heros launch files for Gazebo6 6 | 7 | Sabrina Heerklotz 8 | Jonathan Hechtbauer 9 | 10 | http://wiki.ros.org/Robots/Innok-Heros 11 | 12 | BSD 13 | 14 | catkin 15 | roslaunch 16 | innok_heros_description 17 | robot_state_publisher 18 | xacro 19 | rviz 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/19052514CenterCylin/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19052514CenterCylin 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/19052514EdgeWall/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19052514EdgeWall 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/19052514EdgeWall_2m/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 19052514EdgeWall_2m 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/README.md: -------------------------------------------------------------------------------- 1 | ## PAY ATTENTION! 2 | 3 | Before importing Apriltags.world, you must execute this command: 4 | 5 | `cd models` 6 | 7 | ` source add_path.sh` 8 | 9 | This command can add a correct path and find the correct picture. -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_5x5_plane.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_5x5_plane.tar.gz -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/materials/scripts/Apriltag.material: -------------------------------------------------------------------------------- 1 | material RepeatedApriltag 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | // Relative to the location of the material script 10 | texture ../textures/apriltagMap_5x5_4.9000m.png 11 | // Repeat the texture over the surface 12 | scale 1.0 1.0 13 | } 14 | } 15 | } 16 | } 17 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/materials/textures/apriltagMap_5x5_4.9000m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/materials/textures/apriltagMap_5x5_4.9000m.png -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/materials/textures/apriltagMap_7x7_6.7000m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/materials/textures/apriltagMap_7x7_6.7000m.png -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/materials/textures/apriltagMap_9x9_8.5000m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/materials/textures/apriltagMap_9x9_8.5000m.png -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | apriltags map 5 | 1.0 6 | model.sdf 7 | 8 | 9 | LI Jinjie 10 | lijinjie362@outlook.com 11 | 12 | 13 | 14 | Example of adding the apriltag picture to a plane 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/Apriltags_plane/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 4.9 4.9 .1 10 | 11 | 12 | 13 | 14 | 15 | 16 | 4.9 4.9 .1 17 | 18 | 19 | 20 | 25 | 0.5 0.5 0.5 1 26 | 0.6 0.6 0.6 1 27 | 0 0 0 1 28 | 0 0 0 1 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/add_path.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | ## This shell script is used to add path to my apriltag map. 4 | 5 | # get the absolute path of the current folder. 6 | DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 7 | export GAZEBO_MODEL_PATH=$DIR:$GAZEBO_MODEL_PATH 8 | echo "GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH" 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/apriltagMap_5x5_4.9000m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/apriltagMap_5x5_4.9000m.png -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/apriltagMap_7x7_6.7000m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/apriltagMap_7x7_6.7000m.png -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/apriltagMap_9x9_8.5000m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/Apriltags_world/models/apriltagMap_9x9_8.5000m.png -------------------------------------------------------------------------------- /catkin_ws/src/innok_heros_gazebo/worlds/head_texture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/innok_heros_gazebo/worlds/head_texture.png -------------------------------------------------------------------------------- /catkin_ws/src/judge_system/include/authorized_check.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class AuthorizedCheck { 12 | public: 13 | AuthorizedCheck() {} 14 | ~AuthorizedCheck() { 15 | close(_socket_fd); 16 | } 17 | 18 | int init(const std::string& ip_str, int32_t port); 19 | void gen_request(const std::string& content); 20 | int run_check(); 21 | 22 | private: 23 | void send_request(); 24 | void trans_response(); 25 | int do_check(); 26 | 27 | void split(std::string strtem, char a, std::vector& output_vec); 28 | 29 | std::string _ip_str; 30 | int32_t _port = 0; 31 | int32_t _socket_fd = 0; 32 | struct sockaddr_in _address; 33 | struct hostent* _server_ptr; 34 | std::stringstream _request; 35 | char _buffer[1024 * 1024] = {0}; 36 | }; 37 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system/launch/judge_system_Buav5_Auav8.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system/launch/judge_system_car4_uav6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system/launch/judge_system_car6_uav6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "judge_interface.h" 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "judge_system"); 8 | ros::NodeHandle nh; 9 | JudgeSystem::JudgeInterface::instance()->init_judgement(nh); 10 | ros::Rate r(30); 11 | while(ros::ok()){ 12 | ros::spinOnce(); 13 | JudgeSystem::JudgeInterface::instance()->publish_pose_model_state(); 14 | // JudgeSystem::JudgeInterface::instance()->ensure_dead_uav_down(); 15 | r.sleep(); 16 | } 17 | // ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system/src/main_gui.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "judge_gui.h" 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "judge_system_gui"); 6 | ros::NodeHandle nh; 7 | ros::Rate r(30); 8 | std::string res_path; 9 | if (argc > 1) { 10 | res_path = std::string(argv[1]); 11 | }else 12 | { 13 | res_path = "."; 14 | } 15 | ROS_WARN_STREAM("result save path: " << res_path); 16 | JudgeSystem::JudgeGui plot_gui(nh, res_path); 17 | while(!plot_gui.is_game_over()){ 18 | plot_gui.plot_result(); 19 | ros::spinOnce(); 20 | r.sleep(); 21 | } 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | add_compile_options(-std=c++11) 3 | project(judge_system_msgs) 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | std_msgs 10 | message_generation 11 | ) 12 | 13 | add_message_files( 14 | FILES 15 | Alive_model.msg 16 | Alive_models.msg 17 | Bullet_launch.msg 18 | Move_cmd.msg 19 | Attack_cmd.msg 20 | ) 21 | add_service_files( 22 | FILES 23 | clear_bullets.srv 24 | ) 25 | generate_messages( 26 | DEPENDENCIES 27 | geometry_msgs 28 | std_msgs 29 | ) 30 | 31 | catkin_package( 32 | CATKIN_DEPENDS geometry_msgs std_msgs message_runtime 33 | ) 34 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system_msgs/msg/Alive_model.msg: -------------------------------------------------------------------------------- 1 | std_msgs/String name 2 | std_msgs/Float64 blood 3 | std_msgs/Int32 bullet -------------------------------------------------------------------------------- /catkin_ws/src/judge_system_msgs/msg/Alive_models.msg: -------------------------------------------------------------------------------- 1 | std_msgs/String team 2 | Alive_model[] body -------------------------------------------------------------------------------- /catkin_ws/src/judge_system_msgs/msg/Attack_cmd.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | std_msgs/String team 3 | std_msgs/String name 4 | geometry_msgs/Point start 5 | geometry_msgs/Point target 6 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system_msgs/msg/Bullet_launch.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | std_msgs/String name 3 | geometry_msgs/Point start 4 | geometry_msgs/Point target 5 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system_msgs/msg/Move_cmd.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | std_msgs/String team 3 | std_msgs/String[] name 4 | geometry_msgs/Twist[] cmd 5 | 6 | -------------------------------------------------------------------------------- /catkin_ws/src/judge_system_msgs/srv/clear_bullets.srv: -------------------------------------------------------------------------------- 1 | bool clear 2 | --- 3 | bool ok -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/config/config.yaml: -------------------------------------------------------------------------------- 1 | mobile_base: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 20 6 | 7 | joint1_position_controller: 8 | type: velocity_controllers/JointVelocityController 9 | joint: wheel1 10 | 11 | joint2_position_controller: 12 | type: velocity_controllers/JointVelocityController 13 | joint: wheel2 14 | 15 | joint3_position_controller: 16 | type: velocity_controllers/JointVelocityController 17 | joint: wheel3 18 | 19 | mobile_base_controller: 20 | type: "omni_triangle_controller/OmniDriveController" 21 | # type: base_simple_controller/BaseSimpleController 22 | wheel1: 'wheel_front' 23 | wheel2: 'wheel_left' 24 | wheel3: 'wheel_right' 25 | wheel_radius: 0.0325 26 | wheel_center: 0.161 27 | 28 | # servo1_position_controller: 29 | # type: position_controllers/JointPositionController 30 | # joint: servo_1 31 | # 32 | # servo2_position_controller: 33 | # type: position_controllers/JointPositionController 34 | # joint: servo_2 35 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/config/stone_v3_config.yaml: -------------------------------------------------------------------------------- 1 | mobile_base: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 20 6 | 7 | pitch_position_controller: 8 | type: position_controllers/JointPositionController 9 | joint: pitch_joint 10 | 11 | yaw_position_controller: 12 | type: position_controllers/JointPositionController 13 | joint: yaw_joint 14 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/launch/demo_move_base_amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/launch/multi_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/launch/sc_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/launch/spawn_a_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/launch/spawn_test_single_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/map/magichouse.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sc_gazebo/map/magichouse.pgm -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/map/magichouse.yaml: -------------------------------------------------------------------------------- 1 | image: magichouse.pgm 2 | resolution: 0.050000 3 | origin: [-10.929414, -10.830637, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/map/playground.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sc_gazebo/map/playground.pgm -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/map/playground.yaml: -------------------------------------------------------------------------------- 1 | free_thresh: 0.196 2 | image: playground.pgm 3 | negate: 0 4 | occupied_thresh: 0.65 5 | origin: [-6.8999999999999915, -5.8999999999999915, 0.0] 6 | resolution: 0.05 7 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/map/willowgarage_sc.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sc_gazebo/map/willowgarage_sc.pgm -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/map/willowgarage_sc.yaml: -------------------------------------------------------------------------------- 1 | image: willowgarage_sc.pgm 2 | resolution: 0.050000 3 | origin: [-51.224998, -51.224998, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/materials/textures/kinect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sc_gazebo/materials/textures/kinect.png -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/meshes/base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sc_gazebo/meshes/base_link.stl -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/meshes/mecanum_left_hands.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sc_gazebo/meshes/mecanum_left_hands.stl -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/meshes/mecanum_right_hands.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sc_gazebo/meshes/mecanum_right_hands.stl -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/urdf/base_car.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/urdf/sc0.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /catkin_ws/src/sc_gazebo/urdf/sc_mecanum.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(sim_platform_pysdk) 3 | 4 | find_package(cmake_modules REQUIRED) 5 | find_package(catkin REQUIRED) 6 | catkin_package() 7 | 8 | # catkin_python_setup() 9 | 10 | 11 | install(PROGRAMS src/platform_interface.py 12 | src/example.py 13 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 14 | ) 15 | 16 | install(DIRECTORY launch/ 17 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 18 | ) -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/README.md: -------------------------------------------------------------------------------- 1 | # platform_interface 2 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/launch/start_2AI.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/launch/start_computer_and_your_code.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/package.xml: -------------------------------------------------------------------------------- 1 | 2 | sim_platform_pysdk 3 | 1.0.0 4 | 5 | Controllers developed for the Judge system 6 | 7 | Mike Hamer 8 | 9 | catkin 10 | 11 | cmake_modules 12 | cmake_modules 13 | 14 | BSD 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/common/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | __author__ = 'ecpk-uav-lab' -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/common/obstacles.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | Obstacles = { 4 | 'House1': [-7, 7, -8.3, 8.3], 5 | 'school': [-13, 13, -24, -48] 6 | } -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/computer/AI.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sim_platform_pysdk/src/computer/AI.so -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/computer/computer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from AI import AI 4 | 5 | if __name__ == '__main__': 6 | 7 | ai = AI() 8 | ai.Initialize() 9 | ai.run() -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/computer/computer_interface.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sim_platform_pysdk/src/computer/computer_interface.so -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/computer/mymath.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sim_platform_pysdk/src/computer/mymath.so -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/example.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sim_platform_pysdk/src/player/example.pyc -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/plan_points.py: -------------------------------------------------------------------------------- 1 | # fill blank list if the agent has not target 2 | 3 | Target = { 4 | 'uavA1': [ 5 | [0, 15, 10, 0], 6 | [-10, 10, 10, 0], 7 | [-10, 12, 10, 0], 8 | ], 9 | 'uavA2': [ 10 | [0, 0, 10, 0], 11 | [-10, 0, 10, 0], 12 | [-10, 0, 10, 0], 13 | ], 14 | 'uavA3': [ 15 | [0, -15, 10, 0], 16 | [-10, -10, 10, 0], 17 | [-10, -12, 10, 0], 18 | ], 19 | 'carA1': [ 20 | [0, 10, 0, 0], 21 | [-10, 12, 0, 0], 22 | [-10, 10, 0, 0], 23 | ], 24 | 'carA2': [ 25 | [0, -10, 0, 0], 26 | [-10, -12, 0, 0], 27 | [-10, 10, 0, 0], 28 | ], 29 | 'carA3': [ 30 | [10, 0, 0, 0], 31 | [20, 0, 0, 0], 32 | [10, 0, 0, 0], 33 | ], 34 | } 35 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/plan_points.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sim_platform_pysdk/src/player/plan_points.pyc -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/player.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from example import Example 4 | 5 | if __name__ == "__main__": 6 | 7 | example = Example() 8 | example.set_rate(50) 9 | 10 | while not example.is_shutdown(): 11 | example.process() 12 | example.sleep() 13 | 14 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/player_interface.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/catkin_ws/src/sim_platform_pysdk/src/player/player_interface.so -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/test1move.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | 4 | from player_interface import PlayerInterface 5 | 6 | 7 | 8 | class Example(PlayerInterface): 9 | def __init__(self): #### initialization 10 | super(Example, self).__init__() 11 | 12 | def deal_move(self): #### design the move strategy 13 | angular_z = 0 ####or more simple:cmd = [1,0,0,0] 14 | linear_x = 1 15 | linear_y = 0 16 | linear_z = 0 17 | cmd = [linear_x, linear_y, linear_z, angular_z] 18 | self.set_move_cmd('uavA1', cmd) 19 | cmd=[0,0,0,0] 20 | self.set_move_cmd('uavA2', cmd) 21 | self.set_move_cmd('uavA3', cmd) 22 | self.set_move_cmd('carA1', cmd) 23 | self.set_move_cmd('carA2', cmd) 24 | self.set_move_cmd('carA3', cmd) 25 | self.move_cmd_send() 26 | 27 | def process(self): 28 | self.deal_move() 29 | 30 | 31 | # Coding stop here. 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/test_move_attack.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | 4 | from player_interface import PlayerInterface 5 | 6 | 7 | 8 | class Example(PlayerInterface): 9 | def __init__(self): #### initialization 10 | super(Example, self).__init__() 11 | 12 | def deal_move(self): #### design the move strategy 13 | ####for this, 3move and 2 attack 1 14 | ####3 uav move 15 | for name in self.get_self_agent_name_list(): 16 | if 'uav' in name: 17 | angular_z = 0 18 | linear_x = 1 19 | linear_y = 0 20 | linear_z = 0 21 | 22 | cmd = [linear_x, linear_y, linear_z, angular_z] 23 | self.set_move_cmd(name, cmd) 24 | elif 'car' in name: 25 | cmd=[0,0,0,0] 26 | self.set_move_cmd(name, cmd) 27 | self.move_cmd_send() 28 | 29 | 30 | def deal_attack(self): #### design the attack strategy 31 | ####uavA1 and uavA2 attack carB1 32 | target1 = self.get_pose('carB1')[0:3] #### get the position of CAR B1 of enemy team 33 | 34 | self.attack_cmd_send('uavA1', target1) #### send the attack command 35 | 36 | self.attack_cmd_send('uavA2', target1) 37 | 38 | def process(self): 39 | self.deal_move() 40 | self.deal_attack() 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /catkin_ws/src/sim_platform_pysdk/src/player/testattack.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import math 3 | 4 | from player_interface import PlayerInterface 5 | 6 | 7 | 8 | class Example(PlayerInterface): 9 | def __init__(self): #### initialization 10 | super(Example, self).__init__() 11 | 12 | def deal_attack(self): #### design the attack strategy 13 | target1 = self.get_pose('carB1')[0:3] #### get the position of CAR B1 of enemy team 14 | 15 | self.attack_cmd_send('uavA1', target1) #### send the attack command 16 | 17 | ####self.attack_cmd_send('uavA2', target1) 18 | 19 | ####self.attack_cmd_send('carA1', target1) 20 | 21 | 22 | def process(self): 23 | self.deal_attack() 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | # -------------------------------------------------------------------------------- /docs/maze_easy.vsdx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/docs/maze_easy.vsdx -------------------------------------------------------------------------------- /img/0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/0.png -------------------------------------------------------------------------------- /img/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/1.png -------------------------------------------------------------------------------- /img/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/2.png -------------------------------------------------------------------------------- /img/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/3.png -------------------------------------------------------------------------------- /img/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/4.png -------------------------------------------------------------------------------- /img/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/5.png -------------------------------------------------------------------------------- /img/7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/7.png -------------------------------------------------------------------------------- /img/maze_easy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/maze_easy.png -------------------------------------------------------------------------------- /img/maze_easy_data.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/maze_easy_data.png -------------------------------------------------------------------------------- /img/maze_easy_data_grid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/maze_easy_data_grid.png -------------------------------------------------------------------------------- /img/maze_hard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/maze_hard.png -------------------------------------------------------------------------------- /img/perspective.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/perspective.mp4 -------------------------------------------------------------------------------- /img/test.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/test.mp4 -------------------------------------------------------------------------------- /img/test2.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/test2.mp4 -------------------------------------------------------------------------------- /img/test3.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/test3.mp4 -------------------------------------------------------------------------------- /img/uav_move.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/img/uav_move.mp4 -------------------------------------------------------------------------------- /imu_processing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding=utf-8 3 | ''' 4 | @Author : LI Jinjie 5 | @Date : 2020-03-27 11:24:31 6 | @LastEditors : LI Jinjie 7 | @LastEditTime : 2020-03-28 20:40:10 8 | @Units : None 9 | @Description : file content 10 | @Dependencies : None 11 | @NOTICE : None 12 | ''' 13 | import rospy 14 | from sensor_msgs.msg import Imu 15 | import numpy as np 16 | 17 | imu = Imu() 18 | 19 | 20 | def imu_callback(data): 21 | global imu 22 | imu.header.frame_id = 'uav1/odom' 23 | imu = data 24 | cov = np.eye(3) 25 | cov[0, 0] = np.square(0.1) # (x error)^2 26 | cov[1, 1] = np.square(0.1) # (y error)^2 27 | cov[2, 2] = np.square(0.1) # (z error)^2 28 | imu.linear_acceleration_covariance = tuple(cov.ravel().tolist()) 29 | 30 | 31 | def sub(): 32 | sub = rospy.Subscriber('/uav1/raw_imu', Imu, imu_callback) 33 | 34 | 35 | def pub(): 36 | global imu 37 | rospy.init_node('imu_processing', anonymous=True) 38 | print 'imu_processing_node is running......' 39 | 40 | sub = rospy.Subscriber('/uav1/raw_imu', Imu, imu_callback) 41 | pub = rospy.Publisher('/imu_new', Imu, queue_size=10) 42 | 43 | rate = rospy.Rate(100) # 100hz 44 | while not rospy.is_shutdown(): 45 | pub.publish(imu) 46 | rate.sleep() 47 | 48 | 49 | if __name__ == '__main__': 50 | try: 51 | pub() 52 | except rospy.ROSInterruptException: 53 | pass 54 | -------------------------------------------------------------------------------- /maze_map_data/get_index_from_pos.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/maze_map_data/get_index_from_pos.m -------------------------------------------------------------------------------- /maze_map_data/get_pos_from_map_index.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/maze_map_data/get_pos_from_map_index.m -------------------------------------------------------------------------------- /maze_map_data/maze_map.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-swarm/gazebo_drone_tutorials/a58cbea72e78476925f2a113e5526a6e6d64103d/maze_map_data/maze_map.mat -------------------------------------------------------------------------------- /maze_map_data/test.m: -------------------------------------------------------------------------------- 1 | load maze_map.mat 2 | imshow(maze_map) 3 | %test 4 | % P(1, 1) <==> (20, 20) 5 | % P(400, 1)<==> (-20, 20) 6 | % P(400, 400)<==> (-20, -20) 7 | % P(1, 400)<==> (20, -20) 8 | 9 | get_pos_from_map_index(1, 1) 10 | get_pos_from_map_index(400, 1) 11 | 12 | get_index_from_pos(15.8, -12.0) 13 | -------------------------------------------------------------------------------- /pub_target_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import rospy 4 | # from std_msgs.msg import String 5 | from geometry_msgs.msg import Twist 6 | import math 7 | import time 8 | 9 | 10 | def pub_vel_des_test(): 11 | rospy.init_node('pub_vel_test') 12 | pub_vel_des = rospy.Publisher('/uav1/pose_cmd', Twist, queue_size=1) 13 | # pub_cmd = rospy.Publisher('/cmd_input', Twist, queue_size=1) 14 | rate = rospy.Rate(10) 15 | i = 0 16 | start = time.time() 17 | while not rospy.is_shutdown(): 18 | print(i) 19 | cmd_input = Twist() 20 | t = 6.28/20.0 * (time.time() - start) 21 | cmd_input.linear.x = 10 * math.cos(t) 22 | cmd_input.linear.y = 10 * math.sin(t) 23 | cmd_input.linear.z = 10 24 | cmd_input.angular.x = 0.0 25 | cmd_input.angular.y = 0.0 26 | cmd_input.angular.z = 0.0 27 | 28 | # msg_data = Vector3() 29 | # 30 | # msg_data.x = 5 * math.cos(2*3.14/50*i) 31 | # msg_data.y = 5 * math.sin(2*3.14/50*i) 32 | # msg_data.z = 0.0 33 | i = i+1 34 | pub_vel_des.publish(cmd_input) 35 | # pub_cmd.publish(cmd_input) 36 | rate.sleep() 37 | 38 | if __name__ == '__main__': 39 | try: 40 | pub_vel_des_test() 41 | except rospy.ROSInterruptException: pass -------------------------------------------------------------------------------- /pub_two_drone.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | import rospy 4 | # from std_msgs.msg import String 5 | from geometry_msgs.msg import Twist 6 | import math 7 | import time 8 | 9 | 10 | def pub_vel_des_test(): 11 | rospy.init_node('pub_vel_test') 12 | pub_vel_des = rospy.Publisher('/pose_cmd', Twist, queue_size=1) 13 | # pub_cmd = rospy.Publisher('/cmd_input', Twist, queue_size=1) 14 | rate = rospy.Rate(10) 15 | i = 0 16 | start = time.time() 17 | while not rospy.is_shutdown(): 18 | print(i) 19 | cmd_input = Twist() 20 | t = 6.28/20.0 * (time.time() - start) 21 | cmd_input.linear.x = 6 * math.cos(t) 22 | cmd_input.linear.y = 6 * math.sin(t) 23 | cmd_input.linear.z = 4 24 | cmd_input.angular.x = 0.0 25 | cmd_input.angular.y = 0.0 26 | cmd_input.angular.z = 0.0 27 | 28 | # msg_data = Vector3() 29 | # 30 | # msg_data.x = 5 * math.cos(2*3.14/50*i) 31 | # msg_data.y = 5 * math.sin(2*3.14/50*i) 32 | # msg_data.z = 0.0 33 | i = i+1 34 | pub_vel_des.publish(cmd_input) 35 | # pub_cmd.publish(cmd_input) 36 | rate.sleep() 37 | 38 | if __name__ == '__main__': 39 | try: 40 | pub_vel_des_test() 41 | except rospy.ROSInterruptException: pass -------------------------------------------------------------------------------- /start_maze_ctrl.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ./catkin_ws/ 4 | catkin_make 5 | source $(pwd)/devel/setup.bash 6 | echo "loading gazebo world..." 7 | roslaunch innok_heros_gazebo load_maze_world.launch & 8 | sleep 10 9 | echo "loading uav and car..." 10 | roslaunch hector_quadrotor_gazebo spawn_one_quadrotors_for_maze.launch & 11 | sleep 5 12 | 13 | sleep 30 14 | sh scripts/uav_arm.sh 1 /dev/null 2>&1 & 15 | echo "all uav are ready to takeoff..." 16 | sleep 10 17 | 18 | echo "simulation platform ready..." 19 | sleep 1 20 | echo "simulation platform ready..." 21 | sleep 1 22 | echo "simulation platform ready..." 23 | sleep 1 24 | echo "simulation platform ready..." 25 | sleep 1 26 | echo "simulation platform ready..." 27 | sleep 1 28 | echo "simulation platform ready..." 29 | sleep 1 30 | echo "simulation platform ready..." 31 | sleep 1 32 | echo "simulation platform ready..." 33 | sleep 1 34 | echo "simulation platform ready..." 35 | sleep 1 36 | echo "simulation platform ready..." 37 | wait 38 | 39 | exit 40 | 41 | 42 | -------------------------------------------------------------------------------- /start_maze_hard_ctrl.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ./catkin_ws/ 4 | catkin_make 5 | source $(pwd)/devel/setup.bash 6 | echo "loading gazebo world..." 7 | roslaunch innok_heros_gazebo load_hard_world.launch & 8 | sleep 10 9 | echo "loading uav and car..." 10 | roslaunch hector_quadrotor_gazebo spawn_one_quadrotors_for_maze_hard.launch & 11 | sleep 5 12 | 13 | sleep 30 14 | sh scripts/uav_arm.sh 1 /dev/null 2>&1 & 15 | echo "all uav are ready to takeoff..." 16 | sleep 10 17 | 18 | echo "simulation platform ready..." 19 | sleep 1 20 | echo "simulation platform ready..." 21 | sleep 1 22 | echo "simulation platform ready..." 23 | sleep 1 24 | echo "simulation platform ready..." 25 | sleep 1 26 | echo "simulation platform ready..." 27 | sleep 1 28 | echo "simulation platform ready..." 29 | sleep 1 30 | echo "simulation platform ready..." 31 | sleep 1 32 | echo "simulation platform ready..." 33 | sleep 1 34 | echo "simulation platform ready..." 35 | sleep 1 36 | echo "simulation platform ready..." 37 | wait 38 | 39 | exit 40 | 41 | 42 | -------------------------------------------------------------------------------- /start_maze_hard_hector_with_car.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ./catkin_ws/ 4 | catkin_make 5 | source $(pwd)/devel/setup.bash 6 | echo "loading gazebo world..." 7 | roslaunch innok_heros_gazebo load_hard_world.launch & 8 | sleep 10 9 | echo "loading uav and car..." 10 | roslaunch hector_quadrotor_gazebo spawn_one_quadrotors_for_maze_hard.launch & 11 | sleep 5 12 | 13 | sleep 30 14 | sh scripts/uav_arm.sh 1 /dev/null 2>&1 & 15 | echo "all uav are ready to takeoff..." 16 | sleep 5 17 | roslaunch sc_gazebo add_car_to_world.launch & 18 | sleep 5 19 | python ../judge_uav_solve_maze.py 20 | sleep 5 21 | 22 | echo "simulation platform ready..." 23 | sleep 1 24 | echo "simulation platform ready..." 25 | sleep 1 26 | echo "simulation platform ready..." 27 | sleep 1 28 | echo "simulation platform ready..." 29 | sleep 1 30 | echo "simulation platform ready..." 31 | sleep 1 32 | echo "simulation platform ready..." 33 | sleep 1 34 | echo "simulation platform ready..." 35 | sleep 1 36 | echo "simulation platform ready..." 37 | sleep 1 38 | echo "simulation platform ready..." 39 | sleep 1 40 | echo "simulation platform ready..." 41 | wait 42 | 43 | exit 44 | 45 | 46 | -------------------------------------------------------------------------------- /start_maze_use_car.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ./catkin_ws/ 4 | catkin_make 5 | source $(pwd)/devel/setup.bash 6 | echo "loading gazebo world..." 7 | roslaunch innok_heros_gazebo load_hard_world.launch & 8 | sleep 5 9 | roslaunch sc_gazebo add_car_to_world.launch & 10 | sleep 5 11 | echo "simulation platform ready..." 12 | sleep 1 13 | echo "simulation platform ready..." 14 | sleep 1 15 | echo "simulation platform ready..." 16 | sleep 1 17 | echo "simulation platform ready..." 18 | sleep 1 19 | echo "simulation platform ready..." 20 | sleep 1 21 | echo "simulation platform ready..." 22 | sleep 1 23 | echo "simulation platform ready..." 24 | sleep 1 25 | echo "simulation platform ready..." 26 | sleep 1 27 | echo "simulation platform ready..." 28 | sleep 1 29 | echo "simulation platform ready..." 30 | wait 31 | 32 | exit 33 | 34 | 35 | -------------------------------------------------------------------------------- /start_maze_use_hector.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ./catkin_ws/ 4 | catkin_make 5 | source $(pwd)/devel/setup.bash 6 | echo "loading gazebo world..." 7 | roslaunch innok_heros_gazebo load_hard_world.launch & 8 | sleep 10 9 | echo "loading uav and car..." 10 | roslaunch hector_quadrotor_gazebo spawn_one_quadrotors_for_maze_hard.launch & 11 | sleep 5 12 | 13 | sleep 30 14 | sh scripts/uav_arm.sh 1 /dev/null 2>&1 & 15 | echo "all uav are ready to takeoff..." 16 | sleep 5 17 | 18 | echo "simulation platform ready..." 19 | sleep 1 20 | echo "simulation platform ready..." 21 | sleep 1 22 | echo "simulation platform ready..." 23 | sleep 1 24 | echo "simulation platform ready..." 25 | sleep 1 26 | echo "simulation platform ready..." 27 | sleep 1 28 | echo "simulation platform ready..." 29 | sleep 1 30 | echo "simulation platform ready..." 31 | sleep 1 32 | echo "simulation platform ready..." 33 | sleep 1 34 | echo "simulation platform ready..." 35 | sleep 1 36 | echo "simulation platform ready..." 37 | wait 38 | 39 | exit 40 | 41 | 42 | -------------------------------------------------------------------------------- /start_test_uav1.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ./catkin_ws/ 4 | catkin_make 5 | source $(pwd)/devel/setup.bash 6 | echo "loading gazebo world..." 7 | roslaunch innok_heros_gazebo load_maze_world.launch & 8 | sleep 10 9 | echo "loading uav and car..." 10 | roslaunch hector_quadrotor_gazebo spawn_one_quadrotors.launch & 11 | sleep 5 12 | 13 | sleep 30 14 | sh scripts/uav_arm.sh 1 /dev/null 2>&1 & 15 | echo "all uav are ready to takeoff..." 16 | sleep 10 17 | 18 | echo "simulation platform ready..." 19 | sleep 1 20 | echo "simulation platform ready..." 21 | sleep 1 22 | echo "simulation platform ready..." 23 | sleep 1 24 | echo "simulation platform ready..." 25 | sleep 1 26 | echo "simulation platform ready..." 27 | sleep 1 28 | echo "simulation platform ready..." 29 | sleep 1 30 | echo "simulation platform ready..." 31 | sleep 1 32 | echo "simulation platform ready..." 33 | sleep 1 34 | echo "simulation platform ready..." 35 | sleep 1 36 | echo "simulation platform ready..." 37 | wait 38 | 39 | exit 40 | 41 | 42 | --------------------------------------------------------------------------------