├── scrips ├── simulation │ ├── experiment_test │ ├── experiment_accuracy_noroi │ ├── experiment_accuracy_more │ ├── experiment_accuracy_quick │ ├── experiment_accuracy_full │ ├── experiment_accuracy_extended │ ├── ssd_server.sh │ ├── rossleep.py │ ├── experiment_loss_full │ ├── cleanup.sh │ └── do_experiments.sh ├── robots.txt ├── startvideo.sh ├── stopvideo.sh ├── fixcamera.sh ├── projection.sh ├── transformations.sh ├── camera_config.sh ├── navigation.sh ├── sensorfusion.sh ├── telemetry.sh ├── video.sh ├── diffgps.sh ├── detection.sh ├── roslink.sh ├── roscore.sh ├── cleanup.sh ├── poweroff.sh ├── log.sh ├── logall.sh ├── bandwidth.sh ├── readme.txt └── startup.sh └── packages ├── optional ├── gcs_visualization │ ├── README.md │ ├── src │ │ └── gcs_visualization │ │ │ ├── __init__.py │ │ │ └── world.py │ ├── setup.py │ ├── .gitignore │ └── package.xml ├── basler_image_capture │ └── Grab │ │ ├── src │ │ ├── replay_node.cpp │ │ ├── videograb_node.cpp │ │ ├── frameptr.h │ │ ├── baslerhelper.h │ │ ├── videograb.h │ │ ├── replay.h │ │ └── libavhelper.h │ │ ├── readme.md │ │ ├── extract_images.sh │ │ └── package.xml └── ptgrey_image_capture │ └── Grab │ ├── src │ ├── replay_node.cpp │ ├── videograb_node.cpp │ ├── frameptr.h │ ├── ptgreyhelper.h │ ├── videograb.h │ ├── replay.h │ └── libavhelper.h │ └── package.xml ├── simulation ├── fake_communication_failure │ ├── README.md │ ├── launch │ │ └── default.launch │ ├── bin │ │ └── fake_communication_failure_node │ └── package.xml ├── librepilot_gazebo_bridge │ ├── README.md │ ├── cfg │ │ └── FakeOffset.cfg │ ├── launch │ │ └── default.launch │ └── package.xml ├── random_moving_target │ ├── src │ │ ├── command_vels.mat │ │ ├── random_walk.py │ │ ├── target_robot.cpp │ │ ├── move_robot_randomly.cpp │ │ └── virtual_target_square_motion.cpp │ ├── urdf │ │ └── target_RAL.sdf │ ├── launch │ │ ├── spawn_target_withID.launch │ │ ├── publish_target.launch │ │ ├── move_target_withID.launch │ │ └── spawn_move_target_withID.launch │ ├── CMakeLists.txt │ └── package.xml └── aircap │ ├── launch │ └── recorded_overlay.launch │ └── package.xml ├── flight ├── camera_configs │ ├── config │ │ ├── robots │ │ │ ├── machine_2.yml │ │ │ ├── machine_3.yml │ │ │ └── machine_1.yml │ │ ├── ptgrey │ │ │ ├── info_ptgrey.yaml │ │ │ ├── calibration_ptgrey.yaml │ │ │ ├── calibration_ptgrey.ini │ │ │ └── orbslam_outdoors_ptgrey.yaml │ │ ├── basler │ │ │ ├── info_basler.yaml │ │ │ ├── calibration_basler.yaml │ │ │ ├── calibration_basler.ini │ │ │ └── orbslam_outdoors_basler.yaml │ │ ├── logitech_b500 │ │ │ ├── info_logitech_b500.yaml │ │ │ ├── calibration_logitech_b500.yaml │ │ │ ├── calibration_logitech_b500.ini │ │ │ └── orbslam_settings_logitech_b500.yaml │ │ ├── logitech_c920 │ │ │ ├── info_logitech_c920.yaml │ │ │ ├── calibration_logitech_c920.ini │ │ │ ├── calibration_logitech_c920.yaml │ │ │ └── orbslam_settings_c920.yaml │ │ ├── porthos43 │ │ │ ├── ptgrey_45deg.ini │ │ │ ├── ptgrey_45deg_2.ini │ │ │ ├── porthos43_ptgrey_45deg.ini │ │ │ ├── ptgrey_45deg.yml │ │ │ ├── ptgrey_45deg_2.yml │ │ │ └── porthos43_ptgrey_45deg.yml │ │ ├── porthos44 │ │ │ ├── basler_60deg.ini │ │ │ ├── porthos44_basler_60deg.ini │ │ │ ├── basler_60deg.yml │ │ │ └── porthos44_basler_60deg.yml │ │ ├── porthos45 │ │ │ ├── basler_60deg.ini │ │ │ ├── porthos45_basler_60deg.ini │ │ │ ├── basler_60deg.yml │ │ │ └── porthos45_basler_60deg.yml │ │ └── gazebo.yml │ ├── launch │ │ ├── capture_logitech_b500.launch │ │ ├── capture_logitech_c920.launch │ │ ├── publish_info_gazebo.launch │ │ ├── publish_info_basler.launch │ │ ├── publish_info_ptgrey.launch │ │ ├── publish_info_robot.launch │ │ └── video_stream_ptgrey.launch │ ├── src │ │ └── CameraInfoPublisher.cpp │ └── package.xml ├── nmpc_planner │ ├── cfg │ │ ├── obstacles_real.yaml │ │ ├── obstacles_simulation.yaml │ │ └── nmpcPlannerParams.cfg │ ├── src │ │ └── plotter.py │ ├── include │ │ ├── solver_baseMPC │ │ │ └── OCPsolver.h │ │ ├── solver │ │ │ ├── MPCsolver.h │ │ │ └── util.c │ │ ├── solver_act │ │ │ ├── MPCsolver.h │ │ │ └── description.cvxgen │ │ ├── solver_obstacles_SSRR │ │ │ ├── MPCsolver.h │ │ │ └── description.cvxgen │ │ └── solver_obstacles │ │ │ └── MPCsolver.h │ ├── package.xml │ └── launch │ │ └── planner_activeTracking_square.launch ├── neural_network_detector │ ├── msg │ │ ├── NeuralNetworkNumberOfDetections.msg │ │ ├── NeuralNetworkDetectionArray.msg │ │ ├── NeuralNetworkFeedback.msg │ │ └── NeuralNetworkDetection.msg │ ├── config │ │ └── ssd_300.yaml │ ├── src │ │ ├── neural_network_detector_overlay_only.cpp │ │ └── neural_network_detector_node.cpp │ ├── launch │ │ ├── overlay.launch │ │ └── one_robot.launch │ ├── include │ │ ├── cv │ │ │ └── extensions │ │ │ │ └── projection.h │ │ └── neural_network_detector │ │ │ ├── Overlay.h │ │ │ └── NNDetector.h │ └── package.xml ├── multimaster_setup │ ├── startvideo.sh │ ├── stopvideo.sh │ ├── viewvideo.sh │ ├── quickstart_mm.sh │ └── mm_setup.launch ├── diff_gps │ ├── target.sh │ ├── launch │ │ ├── target.launch │ │ └── diff_gps.launch │ ├── src │ │ └── serialReadlinefunctionCode │ └── package.xml ├── uav_msgs │ ├── CMakeLists.txt │ ├── package.xml │ └── msg │ │ └── uav_pose.msg ├── target_tracker_distributed_kf │ ├── cfg │ │ ├── tracker_configurable_parameters.yaml │ │ └── KalmanFilterParams.cfg │ ├── src │ │ └── target_tracker_distributed_kf_node.cpp │ ├── launch │ │ └── tracker.launch │ └── package.xml ├── projection_models │ └── model_distance_from_height │ │ ├── src │ │ └── models │ │ │ ├── Model2D.cpp │ │ │ └── Model3D.cpp │ │ ├── cfg │ │ └── ModelParameters.cfg │ │ ├── include │ │ └── model_distance_from_height │ │ │ ├── models │ │ │ ├── Model2D.h │ │ │ ├── Model3D.h │ │ │ └── ModelBase.h │ │ │ └── Projector.h │ │ ├── launch │ │ └── one_robot.launch │ │ └── package.xml ├── tf_from_uav_pose │ ├── cfg │ │ └── ReconfigureParams.cfg │ ├── include │ │ └── tf_from_uav_pose │ │ │ └── tf_from_uav_pose.h │ ├── package.xml │ └── launch │ │ └── for_one_robot.launch ├── generic_potential_field │ ├── cfg │ │ └── potentialParams.cfg │ ├── package.xml │ ├── src │ │ ├── TestCalculus.cpp │ │ ├── PotentialFunction.cpp │ │ └── CoTanPotentialFunctions.cpp │ ├── include │ │ ├── CoTanPotentialFunctions.hpp │ │ ├── PotentialFunction.hpp │ │ └── PotentialFunctionImpl.hpp │ └── CMakeLists.txt ├── CMakeModules │ └── FindEigen.cmake └── pose_cov_ops_interface │ ├── package.xml │ └── test │ └── test_interface.cpp └── replay └── video_replay_only └── Grab ├── src ├── replay_node.cpp ├── videograb_node.cpp ├── frameptr.h ├── videograb.h ├── replay.h └── libavhelper.h ├── readme.md ├── extract_images.sh └── package.xml /scrips/simulation/experiment_test: -------------------------------------------------------------------------------- 1 | 2 100 1 2 | -------------------------------------------------------------------------------- /scrips/robots.txt: -------------------------------------------------------------------------------- 1 | porthos43 2 | porthos44 3 | porthos45 4 | -------------------------------------------------------------------------------- /packages/optional/gcs_visualization/README.md: -------------------------------------------------------------------------------- 1 | # gcs\_visualization -------------------------------------------------------------------------------- /scrips/simulation/experiment_accuracy_noroi: -------------------------------------------------------------------------------- 1 | 2 100 9 2 | 4 100 9 3 | -------------------------------------------------------------------------------- /packages/optional/gcs_visualization/src/gcs_visualization/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /packages/simulation/fake_communication_failure/README.md: -------------------------------------------------------------------------------- 1 | # gcs\_visualization -------------------------------------------------------------------------------- /packages/simulation/librepilot_gazebo_bridge/README.md: -------------------------------------------------------------------------------- 1 | # gcs\_visualization -------------------------------------------------------------------------------- /scrips/simulation/experiment_accuracy_more: -------------------------------------------------------------------------------- 1 | 8 100 9 2 | 16 100 9 3 | 24 100 9 4 | -------------------------------------------------------------------------------- /scrips/simulation/experiment_accuracy_quick: -------------------------------------------------------------------------------- 1 | 2 100 3 2 | 5 100 3 3 | 8 100 3 4 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/robots/machine_2.yml: -------------------------------------------------------------------------------- 1 | ../porthos44/basler_60deg.yml -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/robots/machine_3.yml: -------------------------------------------------------------------------------- 1 | ../porthos45/basler_60deg.yml -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/robots/machine_1.yml: -------------------------------------------------------------------------------- 1 | ../porthos43/ptgrey_45deg_2.yml -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/cfg/obstacles_real.yaml: -------------------------------------------------------------------------------- 1 | x_obs: [0, 9, 9] 2 | y_obs: [0,-4,4] 3 | -------------------------------------------------------------------------------- /scrips/startvideo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rostopic pub -v -1 /global/record std_msgs/UInt8 "1" 4 | -------------------------------------------------------------------------------- /scrips/stopvideo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rostopic pub -v -1 /global/record std_msgs/UInt8 "0" 4 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/msg/NeuralNetworkNumberOfDetections.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint16 data -------------------------------------------------------------------------------- /scrips/fixcamera.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | sudo sh -c 'echo 1000 >/sys/module/usbcore/parameters/usbfs_memory_mb' 4 | 5 | -------------------------------------------------------------------------------- /packages/flight/multimaster_setup/startvideo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rostopic pub -v -1 /global/record std_msgs/UInt8 "1" 4 | -------------------------------------------------------------------------------- /packages/flight/multimaster_setup/stopvideo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rostopic pub -v -1 /global/record std_msgs/UInt8 "0" 4 | -------------------------------------------------------------------------------- /scrips/simulation/experiment_accuracy_full: -------------------------------------------------------------------------------- 1 | 2 100 3 2 | 4 100 3 3 | 7 100 3 4 | 1 100 3 5 | 3 100 3 6 | 5 100 3 7 | 6 100 3 8 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/msg/NeuralNetworkDetectionArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | NeuralNetworkDetection[] detections 3 | -------------------------------------------------------------------------------- /scrips/simulation/experiment_accuracy_extended: -------------------------------------------------------------------------------- 1 | 2 100 9 2 | 4 100 9 3 | 7 100 9 4 | 1 100 9 5 | 3 100 9 6 | 5 100 9 7 | 6 100 9 8 | -------------------------------------------------------------------------------- /packages/flight/diff_gps/target.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | nc $1 9000|rosrun diff_gps gps_serial _device_name:=/dev/stdin __name:=$1 _gps_topic:=gpspose 4 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/ptgrey/info_ptgrey.yaml: -------------------------------------------------------------------------------- 1 | rate: 60 2 | image_width: 2448 3 | image_height: 2048 4 | camera_info_url: ./calibration_ptgrey.yaml -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/cfg/obstacles_simulation.yaml: -------------------------------------------------------------------------------- 1 | x_obs: [-14,-15,13,13,-12,-10,-2,3,4,8,12,0,-5] 2 | y_obs: [-7,-3.5,-3,3,6,10,9,9.5,-9,-9,10,5,-7] 3 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/basler/info_basler.yaml: -------------------------------------------------------------------------------- 1 | rate: 60 2 | image_width: 2040 3 | image_height: 1086 4 | camera_info_url: ./calibration_basler.yaml 5 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_b500/info_logitech_b500.yaml: -------------------------------------------------------------------------------- 1 | rate: 30 2 | image_width: 640 3 | image_height: 480 4 | camera_info_url: ./calibration_logitech_b500.yaml 5 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_c920/info_logitech_c920.yaml: -------------------------------------------------------------------------------- 1 | rate: 30 2 | image_width: 1920 3 | image_height: 1080 4 | camera_info_url: ./calibration_logitech_c920.yaml 5 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/src/command_vels.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robot-perception-group/AIRCAP-Legacy/HEAD/packages/simulation/random_moving_target/src/command_vels.mat -------------------------------------------------------------------------------- /scrips/projection.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roslaunch model_distance_from_height one_robot.launch robotID:=$MACHINE 4 | 5 | echo "projection node failed with errorcode $?" >>${LOGDIR}/current/faillog 6 | -------------------------------------------------------------------------------- /scrips/transformations.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roslaunch tf_from_uav_pose for_one_robot.launch robotID:=$MACHINE 4 | 5 | echo "transformation publisher failed with errorcode $?" >>${LOGDIR}/current/faillog 6 | -------------------------------------------------------------------------------- /scrips/camera_config.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roslaunch camera_configs publish_info_robot.launch robotID:=$MACHINE 4 | 5 | echo "camera calibration publisher failed with errorcode $?" >>${LOGDIR}/current/faillog 6 | -------------------------------------------------------------------------------- /scrips/navigation.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roslaunch nmpc_planner planner_activeTracking.launch robotID:=$MACHINE NUM_ROBOTS:=$CCOUNT 4 | 5 | echo "nmpc_planner failed with errorcode $?" >>${LOGDIR}/current/faillog 6 | -------------------------------------------------------------------------------- /scrips/sensorfusion.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roslaunch target_tracker_distributed_kf tracker.launch robotID:=$MACHINE numRobots:=$CCOUNT 4 | 5 | echo "target_tacker_distributed_kf failed with errorcode $?" >>${LOGDIR}/current/faillog 6 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/msg/NeuralNetworkFeedback.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int16 ymin 3 | int16 ymax 4 | int16 xcenter 5 | 6 | #Debug stuff 7 | bool debug_included 8 | int16 head_raw 9 | int16 feet_raw 10 | int16 ycenter -------------------------------------------------------------------------------- /scrips/telemetry.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | mydate=$( date +%s ) 4 | 5 | ../librepilot-mpg/ground/TelemetryBridge/bridge >${LOGDIR}/current/telemetry_${mydate}.opl 6 | 7 | echo "telemetry failed with errorcode $?" >>${LOGDIR}/current/faillog 8 | -------------------------------------------------------------------------------- /packages/flight/multimaster_setup/viewvideo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | machine=1; 4 | if [ ! -z $1 ]; then 5 | machine=$1 6 | fi 7 | 8 | echo "viewing machine $machine" 9 | rosrun image_view image_view image:=/machine_$machine/video 10 | -------------------------------------------------------------------------------- /packages/flight/multimaster_setup/quickstart_mm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | PORT=11311 4 | 5 | if [ $# -eq 1 ]; then 6 | PORT=$1 7 | fi 8 | 9 | export ROS_MASTER_URI=http://localhost:$PORT 10 | 11 | roslaunch mm_setup.launch port:=$PORT --screen 12 | -------------------------------------------------------------------------------- /scrips/video.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosrun videograb videograb_node /machine_$MACHINE ${LOGDIR}/current/ 40 4 __ns:=/machine_$MACHINE __name:=videograb_node_$MACHINE 4 | 5 | echo "video capture and recording failed with errorcode $?" >>${LOGDIR}/current/faillog 6 | -------------------------------------------------------------------------------- /scrips/diffgps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #roslaunch colored_hat_detector segmentation_detection_real.launch robotID:=$MACHINE 4 | roslaunch diff_gps diff_gps.launch robotID:=$MACHINE 5 | 6 | echo "differential GPS failed with with errorcode $?" >>${LOGDIR}/current/faillog 7 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/launch/capture_logitech_b500.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/launch/capture_logitech_c920.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/config/ssd_300.yaml: -------------------------------------------------------------------------------- 1 | desired_resolution: 2 | x: 300 3 | y: 300 4 | 5 | score_threshold: 0.3 6 | desired_class: 15 7 | 8 | variance: 9 | x: 10 | min: .00387 11 | max: .00347 12 | y: 13 | min: .00144 14 | max: .00452 15 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/src/replay_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #include "replay.h" 6 | 7 | int main(int argc, char * *argv) 8 | { 9 | replay::replay replay(argc, argv); 10 | 11 | return replay.run(); 12 | } 13 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/src/replay_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #include "replay.h" 6 | 7 | int main(int argc, char * *argv) 8 | { 9 | replay::replay replay(argc, argv); 10 | 11 | return replay.run(); 12 | } 13 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/src/replay_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #include "replay.h" 6 | 7 | int main(int argc, char * *argv) 8 | { 9 | replay::replay replay(argc, argv); 10 | 11 | return replay.run(); 12 | } 13 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/msg/NeuralNetworkDetection.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | int16 object_class 4 | float32 detection_score 5 | 6 | int16 xmin 7 | int16 xmax 8 | int16 ymin 9 | int16 ymax 10 | 11 | float32 variance_xmin 12 | float32 variance_xmax 13 | float32 variance_ymin 14 | float32 variance_ymax -------------------------------------------------------------------------------- /scrips/detection.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #roslaunch colored_hat_detector segmentation_detection_real.launch robotID:=$MACHINE 4 | roslaunch neural_network_detector one_robot.launch robotID:=$MACHINE host:=192.168.8.2 port:=9900 5 | 6 | echo "neural network detector failed with errorcode $?" >>${LOGDIR}/current/faillog 7 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/src/videograb_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #include "videograb.h" 6 | 7 | int main(int argc, char * *argv) 8 | { 9 | videograb::videograb videograb(argc, argv); 10 | 11 | return videograb.run(); 12 | } 13 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/src/videograb_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #include "videograb.h" 6 | 7 | int main(int argc, char * *argv) 8 | { 9 | videograb::videograb videograb(argc, argv); 10 | 11 | return videograb.run(); 12 | } 13 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/src/videograb_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #include "videograb.h" 6 | 7 | int main(int argc, char * *argv) 8 | { 9 | videograb::videograb videograb(argc, argv); 10 | 11 | return videograb.run(); 12 | } 13 | -------------------------------------------------------------------------------- /scrips/simulation/ssd_server.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ~/src/caffe 4 | id=$1 5 | port=$(( 9900+id )) 6 | while sleep 1; do 7 | ./.build_release/examples/ssd/ssd_server.bin -network_port=$port models/VGGNet/VOC0712/SSD_300x300/deploy.prototxt models/VGGNet/VOC0712/SSD_300x300/VGG_VOC0712_SSD_300x300_iter_120000.caffemodel; 8 | done 9 | -------------------------------------------------------------------------------- /scrips/roslink.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rosrun librepilot librepilot_node /machine_$MACHINE /dev/flightcontroller_serial 115200 __ns:=/machine_$MACHINE __name:=librepilot_node_$MACHINE /machine_$MACHINE/offset:=/machine_$MACHINE/target_tracker/offset 4 | 5 | echo "Librepilot ROS-Link failed with errorcode $?" >>${LOGDIR}/current/faillog 6 | -------------------------------------------------------------------------------- /scrips/roscore.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | mkdir ${LOGDIR}/current/log 4 | rm ${LOGDIR}/log 5 | ln -s ${LOGDIR}/current/log ${LOGDIR}/log 6 | export ROS_MASTER_URI=http://localhost:11311 7 | roslaunch src/flight/multimaster_setup/mm_setup.launch port:=11311 8 | 9 | echo "roslaunch failed with errorcode $?" >>${LOGDIR}/current/faillog 10 | -------------------------------------------------------------------------------- /scrips/simulation/rossleep.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | 4 | import rospy 5 | import os 6 | import sys 7 | 8 | 9 | duration=(float)(sys.argv[1]) 10 | 11 | 12 | rospy.init_node('rossleep', log_level=rospy.INFO) 13 | 14 | rospy.loginfo("sleeping for %f seconds"%duration) 15 | 16 | rospy.sleep(duration) 17 | rospy.loginfo("done") 18 | -------------------------------------------------------------------------------- /scrips/simulation/experiment_loss_full: -------------------------------------------------------------------------------- 1 | 7 0 3 2 | 4 0 3 3 | 2 0 3 4 | 7 0.5 3 5 | 4 0.5 3 6 | 2 0.5 3 7 | 7 1 3 8 | 4 1 3 9 | 2 1 3 10 | 7 3 3 11 | 4 3 3 12 | 2 3 3 13 | 7 6 3 14 | 4 6 3 15 | 2 6 3 16 | 7 12 3 17 | 4 12 3 18 | 2 12 3 19 | 7 25 3 20 | 4 25 3 21 | 2 25 3 22 | 7 50 3 23 | 4 50 3 24 | 2 50 3 25 | 7 75 3 26 | 4 75 3 27 | 2 75 3 28 | 7 90 3 29 | 4 90 3 30 | 2 90 3 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/multimaster_setup/mm_setup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /packages/optional/gcs_visualization/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['gcs_visualization'], 9 | package_dir={'': 'src'}, 10 | ) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /scrips/cleanup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # kill video 4 | killall -INT videograb_node 5 | 6 | # kill rosbag (pose logging) 7 | #killall -INT rosbag 8 | killall -INT record 9 | 10 | # kill telemetry roslink 11 | killall -INT librepilot_node 12 | 13 | # kill telemetry 14 | killall -INT bridge 15 | 16 | # kill the core 17 | #killall -INT roscore 18 | killall -INT roslaunch 19 | 20 | killall -INT bandwidth.sh 21 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/src/neural_network_detector_overlay_only.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 01.09.17. 3 | // 4 | 5 | #include 6 | 7 | int main(int argc, char *argv[]) { 8 | 9 | ros::init(argc, argv, "neural_network_detector_overlay"); 10 | 11 | neural_network_detector::Overlay overlay; 12 | 13 | ros::spin(); 14 | 15 | return EXIT_SUCCESS; 16 | } -------------------------------------------------------------------------------- /packages/flight/uav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uav_msgs) 3 | 4 | find_package(catkin REQUIRED message_generation std_msgs nav_msgs geometry_msgs) 5 | 6 | add_message_files( 7 | FILES 8 | uav_pose.msg 9 | ) 10 | 11 | generate_messages(DEPENDENCIES std_msgs nav_msgs geometry_msgs) 12 | 13 | catkin_package( 14 | CATKIN_DEPENDS message_runtime std_msgs nav_msgs geometry_msgs 15 | ) 16 | 17 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/src/frameptr.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef FRAMEPTR_H 6 | #define FRAMEPTR_H 7 | 8 | #include "boost/thread/mutex.hpp" 9 | 10 | namespace videograb { 11 | class frameptr { 12 | public: 13 | uint64_t number; 14 | uint8_t * buffer; 15 | boost::posix_time::ptime timestamp; 16 | }; 17 | } 18 | 19 | 20 | #endif // ifndef FRAMEPTR_H 21 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/src/frameptr.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef FRAMEPTR_H 6 | #define FRAMEPTR_H 7 | 8 | #include "boost/thread/mutex.hpp" 9 | 10 | namespace videograb { 11 | class frameptr { 12 | public: 13 | uint64_t number; 14 | uint8_t * buffer; 15 | boost::posix_time::ptime timestamp; 16 | }; 17 | } 18 | 19 | 20 | #endif // ifndef FRAMEPTR_H 21 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/src/frameptr.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef FRAMEPTR_H 6 | #define FRAMEPTR_H 7 | 8 | #include "boost/thread/mutex.hpp" 9 | 10 | namespace videograb { 11 | class frameptr { 12 | public: 13 | uint64_t number; 14 | uint8_t * buffer; 15 | boost::posix_time::ptime timestamp; 16 | }; 17 | } 18 | 19 | 20 | #endif // ifndef FRAMEPTR_H 21 | -------------------------------------------------------------------------------- /scrips/simulation/cleanup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # stopping all rosnodes 4 | rosnode kill --all 5 | # stopping the gazebo client aka gui 6 | killall gzclient 7 | # stopping the gazebo server 8 | killall gzserver 9 | killall -9 roslaunch 10 | killall -9 roslaunch 11 | killall -9 roslaunch 12 | killall ssd_server.bin 13 | killall ssd_server.sh 14 | 15 | # lets get a bit more drastic 16 | pkill -f ros/indigo 17 | pkill -f /home/dementor/catkin_ws 18 | -------------------------------------------------------------------------------- /scrips/poweroff.sh: -------------------------------------------------------------------------------- 1 | #/bin/bash 2 | 3 | USERNAME="porthos" 4 | 5 | if [ ! -z $1 ]; then 6 | echo: Usage: $0 7 | echo Turns off all copters listed in robots.txt 8 | exit -1 9 | fi 10 | 11 | echo 12 | echo "Turning off all copters:" 13 | number=0 14 | copters="$( cat robots.txt )" 15 | for copter in $copters; do 16 | echo "$copter :" 17 | number=$(( number+1 )) 18 | ssh $USERNAME@$copter "sudo /sbin/shutdown -h now" 19 | done 20 | 21 | echo 22 | echo "All Done" 23 | 24 | -------------------------------------------------------------------------------- /packages/simulation/librepilot_gazebo_bridge/cfg/FakeOffset.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "librepilot_gazebo_bridge" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("offsetX", double_t, 0, "offsetX", 0.0, -10.0, 10.0) 9 | gen.add("offsetY", double_t, 0, "offsetY", 0.0, -10.0, 10.0) 10 | gen.add("offsetZ", double_t, 0, "offsetZ", 0.0, -10.0, 10.0) 11 | 12 | exit(gen.generate(PACKAGE, "librepilot_gazebo_bridge", "FakeOffsetParams")) 13 | -------------------------------------------------------------------------------- /packages/simulation/librepilot_gazebo_bridge/launch/default.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /packages/flight/target_tracker_distributed_kf/cfg/tracker_configurable_parameters.yaml: -------------------------------------------------------------------------------- 1 | noisePosXVar: 0.0 2 | noisePosYVar: 0.0 3 | noisePosZVar: 0.0 4 | noiseVelXVar: 0.5 5 | noiseVelYVar: 0.5 6 | noiseVelZVar: 0.1 7 | noiseOffXVar: 0.02 8 | noiseOffYVar: 0.02 9 | noiseOffZVar: 0.02 10 | velocityDecayTime: 3.0 11 | offsetDecayTime: 30.0 12 | initialUncertaintyPosXY: 100.0 13 | initialUncertaintyPosZ: 10.0 14 | initialUncertaintyVelXY: 1.0 15 | initialUncertaintyVelZ: 0.5 16 | initialUncertaintyOffsetXY: 1.0 17 | initialUncertaintyOffsetZ: 3.0 18 | -------------------------------------------------------------------------------- /packages/flight/target_tracker_distributed_kf/src/target_tracker_distributed_kf_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 23.05.17. 3 | // 4 | 5 | #include 6 | // #include 7 | 8 | int main(int argc, char* argv[]){ 9 | 10 | ros::init(argc, argv, "target_tracker_kf"); 11 | target_tracker_distributed_kf::DistributedKF3D tracker; 12 | // target_tracker_self_kf::selfKF3D tracker_self; 13 | ros::spin(); 14 | 15 | return EXIT_SUCCESS; 16 | } 17 | -------------------------------------------------------------------------------- /packages/flight/diff_gps/launch/target.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/src/neural_network_detector_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 01.09.17. 3 | // 4 | 5 | #include 6 | 7 | int main(int argc, char *argv[]) { 8 | 9 | ros::init(argc, argv, "neural_network_detector"); 10 | 11 | if (argc < 3) { 12 | ROS_WARN("Usage: rosrun neural_network_detector neural_network_detector_node HOST PORT"); 13 | return EXIT_FAILURE; 14 | } 15 | 16 | neural_network_detector::NNDetector nn_detector(argv[1], argv[2]); 17 | 18 | ros::spin(); 19 | 20 | return EXIT_SUCCESS; 21 | } -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/src/models/Model2D.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 10.05.17. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | namespace model_distance_from_height{ 9 | 10 | Model2D::Model2D(int fy, int cy, double hmean) : fy_(fy), cy_(cy), height_model_(hmean){}; 11 | 12 | double Model2D::compute_distance(double pitch, double height_pixels, double center_pixels){ 13 | return fy_ * height_model_.mean * cos(pitch) / height_pixels * sqrt(1 + (center_pixels-cy_)/fy_ ); 14 | } 15 | 16 | } -------------------------------------------------------------------------------- /packages/flight/diff_gps/launch/diff_gps.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/basler/calibration_basler.yaml: -------------------------------------------------------------------------------- 1 | image_width: 2040 2 | image_height: 1086 3 | camera_name: basler 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1487.006175, 0, 1022.805235, 0, 1487.708688, 593.765679, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.103322, 0.190644, 0.000803, -0.001449, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1499.691895, 0, 1019.600995, 0, 0, 1500.14563, 593.314533, 0, 0, 0, 1, 0] 21 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/ptgrey/calibration_ptgrey.yaml: -------------------------------------------------------------------------------- 1 | image_width: 2448 2 | image_height: 2048 3 | camera_name: ptgrey 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [2406.442617, 0, 1202.854654, 0, 2412.285633, 1021.264401, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.107492, 0.199451, -0.000891, 0.000976, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [2388.249756, 0, 1205.755648, 0, 0, 2396.078125, 1019.648151, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /packages/flight/camera_configs/launch/publish_info_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_c920/calibration_logitech_c920.ini: -------------------------------------------------------------------------------- 1 | # Camera intrinsics 2 | 3 | [image] 4 | 5 | width 6 | 1920 7 | 8 | height 9 | 1080 10 | 11 | [logitech_c920] 12 | 13 | camera matrix 14 | 1414.33708 0.00000 940.69096 15 | 0.00000 1413.58507 559.46813 16 | 0.00000 0.00000 1.00000 17 | 18 | distortion 19 | 0.13335 -0.19215 0.00017 -0.00712 0.00000 20 | 21 | 22 | rectification 23 | 1.00000 0.00000 0.00000 24 | 0.00000 1.00000 0.00000 25 | 0.00000 0.00000 1.00000 26 | 27 | projection 28 | 1442.78577 0.00000 926.24011 0.00000 29 | 0.00000 1455.76721 559.65815 0.00000 30 | 0.00000 0.00000 1.00000 0.00000 31 | -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/cfg/ModelParameters.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "model_distance_from_height" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("height_model_mean", double_t, 0, "Mean", 1.8, 0.5, 2.5) 9 | gen.add("height_model_var", double_t, 0, "Variance", 1.0, 0.0, 50.0) 10 | 11 | gen.add("uncertainty_scale_head", double_t, 0, "Head Uncertainty Scale", 1.0, 0.0, 50.0) 12 | gen.add("uncertainty_scale_feet", double_t, 0, "Feet Uncertainty Scale", 1.0, 0.0, 50.0) 13 | 14 | exit(gen.generate(PACKAGE, "model", "ModelParameters")) -------------------------------------------------------------------------------- /scrips/log.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | mydate=$( date +%s ) 4 | 5 | rosbag record -O ${LOGDIR}/current/rosbag_self_${mydate} /machine_$MACHINE/TransmitterInfo /machine_$MACHINE/pose /machine_$MACHINE/pose/raw /machine_$MACHINE/command /machine_$MACHINE/Imu /machine_$MACHINE/gyrobias /machine_$MACHINE/object_detections /machine_$MACHINE/object_detections/amount /machine_$MACHINE/object_detections/feedback /machine_$MACHINE/object_detections/projected_to_world /machine_$MACHINE/target_tracker/pose /machine_$MACHINE/target_tracker/offset /machine_$MACHINE/diff_gps/groundtruth/gpspose 6 | 7 | echo "local logging failed with errorcode $?" >>${LOGDIR}/current/faillog 8 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/ptgrey/calibration_ptgrey.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2448 8 | 9 | height 10 | 2048 11 | 12 | [ptgrey] 13 | 14 | camera matrix 15 | 2406.442617 0.000000 1202.854654 16 | 0.000000 2412.285633 1021.264401 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.107492 0.199451 -0.000891 0.000976 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 2388.249756 0.000000 1205.755648 0.000000 29 | 0.000000 2396.078125 1019.648151 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/launch/publish_info_basler.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/launch/publish_info_ptgrey.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/src/baslerhelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef BASLERHELPER_H 6 | #define BASLERHELPER_H 7 | 8 | #include "boost/thread/mutex.hpp" 9 | #include "frameptr.h" 10 | 11 | namespace videograb { 12 | class baslerhelper_priv; 13 | 14 | class baslerhelper { 15 | public: 16 | baslerhelper(int framerate); 17 | ~baslerhelper(); 18 | frameptr getFrame (void); 19 | int getHeight(void); 20 | int getWidth(void); 21 | 22 | private: 23 | baslerhelper_priv *instance; 24 | }; 25 | 26 | } 27 | 28 | #endif // ifndef BASLERHELPER_H 29 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/basler/calibration_basler.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2040 8 | 9 | height 10 | 1086 11 | 12 | [basler] 13 | 14 | camera matrix 15 | 1487.006175 0.000000 1022.805235 16 | 0.000000 1487.708688 593.765679 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.103322 0.190644 0.000803 -0.001449 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 1499.691895 0.000000 1019.600995 0.000000 29 | 0.000000 1500.145630 593.314533 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_b500/calibration_logitech_b500.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: logitech_b500 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [567.250017, 0, 319.06103, 0, 568.046005, 211.211007, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.004553, -0.015001, 0.002478, -0.004510999999999999, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [565.750183, 0, 316.476989, 0, 0, 568.704224, 211.978135, 0, 0, 0, 1, 0] 21 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/launch/publish_info_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos43/ptgrey_45deg.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2448 8 | 9 | height 10 | 2048 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 3428.752959 0.000000 1274.310414 16 | 0.000000 3429.943051 1045.353139 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.341183 -0.067376 0.001476 -0.001787 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 3093.501270 0.000000 1278.414051 0.000000 29 | 0.000000 3093.676289 1050.892875 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos43/ptgrey_45deg_2.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2448 8 | 9 | height 10 | 2048 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 3602.380733 0.000000 1208.348894 16 | 0.000000 3615.060635 992.675623 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.371516 0.019931 -0.000375 0.002571 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 3433.480713 0.000000 1210.567765 0.000000 29 | 0.000000 3500.246826 990.126276 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos44/basler_60deg.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2040 8 | 9 | height 10 | 1086 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 1482.342684 0.000000 1004.669692 16 | 0.000000 1486.354117 583.260422 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.111144 0.182102 -0.000953 -0.001470 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 1465.954102 0.000000 1002.618435 0.000000 29 | 0.000000 1460.225098 582.634933 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos45/basler_60deg.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2040 8 | 9 | height 10 | 1086 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 1491.380622 0.000000 1041.123623 16 | 0.000000 1493.154224 594.247698 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.128357 0.205574 0.002959 0.001020 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 1470.154663 0.000000 1041.826056 0.000000 29 | 0.000000 1461.818481 596.901325 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_b500/calibration_logitech_b500.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 640 8 | 9 | height 10 | 480 11 | 12 | [logitech_b500] 13 | 14 | camera matrix 15 | 567.250017 0.000000 319.061030 16 | 0.000000 568.046005 211.211007 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.004553 -0.015001 0.002478 -0.004511 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 565.750183 0.000000 316.476989 0.000000 29 | 0.000000 568.704224 211.978135 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos43/porthos43_ptgrey_45deg.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2448 8 | 9 | height 10 | 2048 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 3428.752959 0.000000 1274.310414 16 | 0.000000 3429.943051 1045.353139 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.341183 -0.067376 0.001476 -0.001787 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 3093.501270 0.000000 1278.414051 0.000000 29 | 0.000000 3093.676289 1050.892875 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos44/porthos44_basler_60deg.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2040 8 | 9 | height 10 | 1086 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 1491.380622 0.000000 1041.123623 16 | 0.000000 1493.154224 594.247698 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.128357 0.205574 0.002959 0.001020 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 1470.154663 0.000000 1041.826056 0.000000 29 | 0.000000 1461.818481 596.901325 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos45/porthos45_basler_60deg.ini: -------------------------------------------------------------------------------- 1 | # oST version 5.0 parameters 2 | 3 | 4 | [image] 5 | 6 | width 7 | 2040 8 | 9 | height 10 | 1086 11 | 12 | [narrow_stereo] 13 | 14 | camera matrix 15 | 1482.342684 0.000000 1004.669692 16 | 0.000000 1486.354117 583.260422 17 | 0.000000 0.000000 1.000000 18 | 19 | distortion 20 | -0.111144 0.182102 -0.000953 -0.001470 0.000000 21 | 22 | rectification 23 | 1.000000 0.000000 0.000000 24 | 0.000000 1.000000 0.000000 25 | 0.000000 0.000000 1.000000 26 | 27 | projection 28 | 1465.954102 0.000000 1002.618435 0.000000 29 | 0.000000 1460.225098 582.634933 0.000000 30 | 0.000000 0.000000 1.000000 0.000000 31 | 32 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/src/ptgreyhelper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef PTGREYHELPER_H 6 | #define PTGREYHELPER_H 7 | 8 | #include "boost/thread/mutex.hpp" 9 | #include "frameptr.h" 10 | 11 | namespace videograb { 12 | class ptgreyhelper_priv; 13 | 14 | class ptgreyhelper { 15 | public: 16 | ptgreyhelper(int framerate); 17 | ~ptgreyhelper(); 18 | frameptr getFrame (uint8_t* buffer, size_t maxsize); 19 | int getHeight(void); 20 | int getWidth(void); 21 | 22 | private: 23 | ptgreyhelper_priv *instance; 24 | }; 25 | 26 | } 27 | 28 | #endif // ifndef PTGREYHELPER_H 29 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/gazebo.yml: -------------------------------------------------------------------------------- 1 | image_width: 2040 2 | image_height: 1086 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1470.3299363953429, 0.0, 1020.5, 0.0, 1470.3299363953429, 543.5, 0.0, 0.0, 1.0] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.0, 0.0, 0.0, 0.0, 0.0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1470.3299363953429, 0.0, 1020.5, -0.0, 0.0, 1470.3299363953429, 543.5, 0.0, 0.0, 0.0, 1.0, 0.0] 21 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/src/videograb.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef VIDEOGRAB_H 6 | #define VIDEOGRAB_H 7 | 8 | #include "ros/ros.h" 9 | #include "boost/thread/mutex.hpp" 10 | #include 11 | 12 | namespace videograb { 13 | class videograb_priv; 14 | 15 | class videograb { 16 | public: 17 | videograb(int argc, char * *argv); 18 | ~videograb(); 19 | int run(void); 20 | void rosinfoPrint(const char *bla); 21 | void videoCallback(const sensor_msgs::ImageConstPtr & msgp); 22 | 23 | private: 24 | videograb_priv *instance; 25 | }; 26 | 27 | } 28 | 29 | #endif // ifndef VIDEOGRAB_H 30 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/src/videograb.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef VIDEOGRAB_H 6 | #define VIDEOGRAB_H 7 | 8 | #include "ros/ros.h" 9 | #include "boost/thread/mutex.hpp" 10 | 11 | namespace videograb { 12 | class videograb_priv; 13 | 14 | class videograb { 15 | public: 16 | videograb(int argc, char * *argv); 17 | ~videograb(); 18 | int run(void); 19 | boost::posix_time::ptime *getStart(void); 20 | boost::posix_time::ptime *getCurrent(void); 21 | void rosinfoPrint(const char *bla); 22 | 23 | private: 24 | videograb_priv *instance; 25 | }; 26 | 27 | } 28 | 29 | #endif // ifndef VIDEOGRAB_H 30 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/src/videograb.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef VIDEOGRAB_H 6 | #define VIDEOGRAB_H 7 | 8 | #include "ros/ros.h" 9 | #include "boost/thread/mutex.hpp" 10 | 11 | namespace videograb { 12 | class videograb_priv; 13 | 14 | class videograb { 15 | public: 16 | videograb(int argc, char * *argv); 17 | ~videograb(); 18 | int run(void); 19 | boost::posix_time::ptime *getStart(void); 20 | boost::posix_time::ptime *getCurrent(void); 21 | void rosinfoPrint(const char *bla); 22 | 23 | private: 24 | videograb_priv *instance; 25 | }; 26 | 27 | } 28 | 29 | #endif // ifndef VIDEOGRAB_H 30 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/src/plotter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import rospy 4 | from uav_msgs.msg import uav_pose 5 | from matplotlib import pyplot as plt 6 | 7 | def callback(data): 8 | global counter 9 | counter += 1 10 | #rospy.loginfo(data.position) 11 | # if counter%10 == 0: 12 | # plt.plot(data.position.x, data.position.y, '*') 13 | # plt.axis([-20,20,-20,20]) 14 | # plt.draw() 15 | # plt.pause(0.00000000001) 16 | 17 | 18 | def listener(): 19 | rospy.init_node('custom_plotter'); 20 | topic = '/waypoint_firefly_'+str(sys.argv[1]) 21 | rospy.Subscriber(topic, uav_pose, callback) 22 | 23 | rospy.spin() 24 | 25 | if __name__ == '__main__': 26 | counter = 0; 27 | listener() -------------------------------------------------------------------------------- /scrips/logall.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | mydate=$( date +%s ) 4 | 5 | rosbag record -O ${LOGDIR}/current/rosbag_all_${mydate} \ 6 | /machine_1/pose /machine_1/object_detections/amount /machine_1/object_detections/projected_to_world \ 7 | /machine_2/pose /machine_2/object_detections/amount /machine_2/object_detections/projected_to_world \ 8 | /machine_3/pose /machine_3/object_detections/amount /machine_3/object_detections/projected_to_world \ 9 | /machine_4/pose /machine_4/object_detections/amount /machine_4/object_detections/projected_to_world \ 10 | /machine_5/pose /machine_5/object_detections/amount /machine_5/object_detections/projected_to_world \ 11 | 12 | echo "remote logging failed with errorcode $?" >>${LOGDIR}/current/faillog 13 | -------------------------------------------------------------------------------- /scrips/bandwidth.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | interface="wlan0" 4 | mydate=$( date +%s ) 5 | filename=${LOGDIR}/current/bandwidthlog_${mydate}.log 6 | 7 | ibytes=$( cat /proc/net/dev |grep $interface |sed -e 's/ \+/ /g' |cut -d ' ' -f 3 ) 8 | obytes=$( cat /proc/net/dev |grep $interface |sed -e 's/ \+/ /g' |cut -d ' ' -f 11 ) 9 | oibytes=$ibytes 10 | oobytes=$obytes 11 | 12 | while sleep 1; do 13 | mydate="$( date +%s )" 14 | ibytes=$( cat /proc/net/dev |grep $interface |sed -e 's/ \+/ /g' |cut -d ' ' -f 3 ) 15 | obytes=$( cat /proc/net/dev |grep $interface |sed -e 's/ \+/ /g' |cut -d ' ' -f 11 ) 16 | echo $mydate $ibytes $obytes $(( ibytes-oibytes)) $(( obytes-oobytes )) >>$filename 17 | oibytes=$ibytes 18 | oobytes=$obytes 19 | done 20 | 21 | -------------------------------------------------------------------------------- /packages/flight/tf_from_uav_pose/cfg/ReconfigureParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "tf_from_uav_pose" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("throttleRate", double_t, 0, "throttleRate", 10, 0, 1000) 9 | 10 | gen.add("offsetX", double_t, 0, "offsetX", 0, -10, 10) 11 | gen.add("offsetY", double_t, 0, "offsetY", 0, -10, 10) 12 | gen.add("offsetZ", double_t, 0, "offsetZ", 0, -10, 10) 13 | 14 | gen.add("covarianceX", double_t, 0, "covarianceX", 0, 0, 5) 15 | gen.add("covarianceY", double_t, 0, "covarianceY", 0, 0, 5) 16 | gen.add("covarianceZ", double_t, 0, "covarianceZ", 0, 0, 5) 17 | 18 | exit(gen.generate(PACKAGE, "tf_from_uav_pose", "ReconfigureParams")) 19 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/src/replay.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef REPLAY_H 6 | #define REPLAY_H 7 | 8 | #include "ros/ros.h" 9 | #include "boost/thread/mutex.hpp" 10 | 11 | namespace replay { 12 | class replay_priv; 13 | 14 | class replay { 15 | public: 16 | replay(int argc, char * *argv); 17 | ~replay(); 18 | int run(void); 19 | boost::posix_time::ptime *getStart(void); 20 | boost::posix_time::ptime *getCurrent(void); 21 | void rosinfoPrint(const char *bla); 22 | 23 | private: 24 | replay_priv *instance; 25 | unsigned long long corrected_diff(boost::posix_time::time_duration diff); 26 | }; 27 | 28 | } 29 | 30 | #endif // ifndef REPLAY_H 31 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/src/replay.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef REPLAY_H 6 | #define REPLAY_H 7 | 8 | #include "ros/ros.h" 9 | #include "boost/thread/mutex.hpp" 10 | 11 | namespace replay { 12 | class replay_priv; 13 | 14 | class replay { 15 | public: 16 | replay(int argc, char * *argv); 17 | ~replay(); 18 | int run(void); 19 | boost::posix_time::ptime *getStart(void); 20 | boost::posix_time::ptime *getCurrent(void); 21 | void rosinfoPrint(const char *bla); 22 | 23 | private: 24 | replay_priv *instance; 25 | unsigned long long corrected_diff(boost::posix_time::time_duration diff); 26 | }; 27 | 28 | } 29 | 30 | #endif // ifndef REPLAY_H 31 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/src/replay.h: -------------------------------------------------------------------------------- 1 | /* 2 | * TODO: Add apropriate License and copyright header 3 | */ 4 | 5 | #ifndef REPLAY_H 6 | #define REPLAY_H 7 | 8 | #include "ros/ros.h" 9 | #include "boost/thread/mutex.hpp" 10 | 11 | namespace replay { 12 | class replay_priv; 13 | 14 | class replay { 15 | public: 16 | replay(int argc, char * *argv); 17 | ~replay(); 18 | int run(void); 19 | boost::posix_time::ptime *getStart(void); 20 | boost::posix_time::ptime *getCurrent(void); 21 | void rosinfoPrint(const char *bla); 22 | 23 | private: 24 | replay_priv *instance; 25 | unsigned long long corrected_diff(boost::posix_time::time_duration diff); 26 | }; 27 | 28 | } 29 | 30 | #endif // ifndef REPLAY_H 31 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/urdf/target_RAL.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0 0 0 0 7 | 8 | walk.dae 9 | 10 | 11 | walk.dae 12 | true 13 | 14 | 15 | 16 | 0 -5 1.2138 17 | 1.15 18 | 1.8 19 | 5.1 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos45/basler_60deg.yml: -------------------------------------------------------------------------------- 1 | image_width: 2040 2 | image_height: 1086 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1491.380622, 0.000000, 1041.123623, 0.000000, 1493.154224, 594.247698, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.128357, 0.205574, 0.002959, 0.001020, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1470.154663, 0.000000, 1041.826056, 0.000000, 0.000000, 1461.818481, 596.901325, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos43/ptgrey_45deg.yml: -------------------------------------------------------------------------------- 1 | image_width: 2448 2 | image_height: 2048 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [3428.752959, 0.000000, 1274.310414, 0.000000, 3429.943051, 1045.353139, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.341183, -0.067376, 0.001476, -0.001787, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [3093.501270, 0.000000, 1278.414051, 0.000000, 0.000000, 3093.676289, 1050.892875, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos43/ptgrey_45deg_2.yml: -------------------------------------------------------------------------------- 1 | image_width: 2448 2 | image_height: 2048 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [3602.380733, 0.000000, 1208.348894, 0.000000, 3615.060635, 992.675623, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.371516, 0.019931, -0.000375, 0.002571, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [3433.480713, 0.000000, 1210.567765, 0.000000, 0.000000, 3500.246826, 990.126276, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos44/basler_60deg.yml: -------------------------------------------------------------------------------- 1 | image_width: 2040 2 | image_height: 1086 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1482.342684, 0.000000, 1004.669692, 0.000000, 1486.354117, 583.260422, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.111144, 0.182102, -0.000953, -0.001470, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1465.954102, 0.000000, 1002.618435, 0.000000, 0.000000, 1460.225098, 582.634933, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /scrips/readme.txt: -------------------------------------------------------------------------------- 1 | globalstart.txt is supposed to be run from a basestation/laptop 2 | 3 | It will start everything up on the robots (listed in robots.txt) all connected in a Wifi wireless LAN. 4 | Each robot should have each other in /etc/hosts 5 | 6 | All relevant ROS nodes and flight control run on the flying robots. 7 | 8 | The Ground Station can be used for monitoring and control but takes no active part in swarm operation! 9 | 10 | 11 | 12 | Script location: 13 | 14 | 15 | The catkin workspace is expected to be in 16 | 17 | 18 | ~/src/catkin_ws/ 19 | 20 | 21 | all scripts in this directory are expected to reside in ~/src/catkin_ws for execution. 22 | 23 | If you have a different catkin workspace directory, please change startup.sh accordingly 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos44/porthos44_basler_60deg.yml: -------------------------------------------------------------------------------- 1 | image_width: 2040 2 | image_height: 1086 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1491.380622, 0.000000, 1041.123623, 0.000000, 1493.154224, 594.247698, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.128357, 0.205574, 0.002959, 0.001020, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1470.154663, 0.000000, 1041.826056, 0.000000, 0.000000, 1461.818481, 596.901325, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos43/porthos43_ptgrey_45deg.yml: -------------------------------------------------------------------------------- 1 | image_width: 2448 2 | image_height: 2048 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [3428.752959, 0.000000, 1274.310414, 0.000000, 3429.943051, 1045.353139, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.341183, -0.067376, 0.001476, -0.001787, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [3093.501270, 0.000000, 1278.414051, 0.000000, 0.000000, 3093.676289, 1050.892875, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/porthos45/porthos45_basler_60deg.yml: -------------------------------------------------------------------------------- 1 | image_width: 2040 2 | image_height: 1086 3 | camera_name: narrow_stereo 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1482.342684, 0.000000, 1004.669692, 0.000000, 1486.354117, 583.260422, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.111144, 0.182102, -0.000953, -0.001470, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1465.954102, 0.000000, 1002.618435, 0.000000, 0.000000, 1460.225098, 582.634933, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /packages/flight/uav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uav_msgs 4 | 0.0.0 5 | The uav_msgs package containing messages for communication within ROS-packages 6 | 7 | Aamir Ahmad 8 | 9 | GPLv3 10 | 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | nav_msgs 17 | geometry_msgs 18 | 19 | message_runtime 20 | std_msgs 21 | nav_msgs 22 | geometry_msgs 23 | 24 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_c920/calibration_logitech_c920.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1920 2 | image_height: 1080 3 | camera_name: logitech_c920 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [1414.337085, 0.000000, 940.690963, 0.000000, 1413.585074, 559.468131, 0.000000, 0.000000, 1.000000] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.133346, -0.192149, 0.000166, -0.007123, 0.000000] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [1442.785767, 0.000000, 926.240111, 0.000000, 0.000000, 1455.767212, 559.658148, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] 21 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/cfg/potentialParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "generic_potential_field" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | 9 | # max distance to feel the obstacle potential 10 | gen.add("tPotFuncZeroD", double_t, 0, "tPotFuncZeroD", 0.5, 0, 5) 11 | 12 | 13 | # min distance to feel the obstacle potential 14 | gen.add("tPotFuncInfD", double_t, 0, "tPotFuncInfD",0.2, 0, 5) 15 | 16 | # max value of the potential function 17 | gen.add("tPotFuncSatValue", double_t, 0, "tPotFuncSatValue",4.0, 0, 15) 18 | 19 | 20 | # gain to scale the potential value 21 | gen.add("tPotFuncGain", double_t, 0, "tPotFuncGain", 100, 0, 500) 22 | 23 | exit(gen.generate(PACKAGE, "generic_potential_field", "potentialParams")) 24 | 25 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/readme.md: -------------------------------------------------------------------------------- 1 | Grab: 2 | 3 | This is a program adapted from a sample code provided in Pylon software. 'Pylon' is the official driver for basler cameras which comes for free and has a number of utilities. However, it is not fully open source. 4 | 5 | 6 | Compilation Instructions: 7 | 8 | Switch to directory libs (3 levels up from Grab and in the same level as the 'lateral' directory). Uncompress pylon-5.0.1.6388-x86_64.tar.gz from 'third_party' folder and follow its installation instructions. 9 | 10 | NOTE: This procedure is same for the server board SBCs (planned for octocopters) 11 | 12 | NOTE2: You might need a newer version of ffmpeg, and might need to compile from source: 13 | sudo apt-get install yasm 14 | git clone https://git.ffmpeg.org/ffmpeg.git ffmpeg 15 | cd ffmpeg 16 | ./configure --enable-shared 17 | make 18 | sudo make install 19 | -------------------------------------------------------------------------------- /packages/simulation/fake_communication_failure/launch/default.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/readme.md: -------------------------------------------------------------------------------- 1 | Grab: 2 | 3 | This is a program adapted from a sample code provided in Pylon software. 'Pylon' is the official driver for basler cameras which comes for free and has a number of utilities. However, it is not fully open source. 4 | 5 | 6 | Compilation Instructions: 7 | 8 | Switch to directory libs (3 levels up from Grab and in the same level as the 'lateral' directory). Uncompress pylon-5.0.1.6388-x86_64.tar.gz from 'third_party' folder and follow its installation instructions. 9 | 10 | NOTE: This procedure is same for the server board SBCs (planned for octocopters) 11 | 12 | NOTE2: You might need a newer version of ffmpeg, and might need to compile from source: 13 | sudo apt-get install yasm 14 | git clone https://git.ffmpeg.org/ffmpeg.git ffmpeg 15 | cd ffmpeg 16 | ./configure --enable-shared 17 | make 18 | sudo make install 19 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/launch/spawn_target_withID.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/extract_images.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | avifile="$1" 4 | timesfile="${1}.times" 5 | 6 | if [ ! \( -e "$avifile" -a -e "$timesfile" \) ]; then 7 | echo "Usage: $0 capture_.xxxxxx.avi" 8 | echo "will extract images from avi file with timestamp encoded in filename" 9 | exit 10 | fi 11 | 12 | echo extracting images from avi 13 | ffmpeg -i "$avifile" -vsync 0 -start_number 0 "${avifile}.tmp_%08d.tiff" 14 | 15 | echo "renaming frames" 16 | count=0; 17 | cat $timesfile | while read framenum frametime; do 18 | filenum="$( printf "%08d" $(( count )) )" 19 | count=$(( count + 1 )) 20 | secs=$(( frametime/1000000 )) 21 | usecs=$( printf "%06d" $(( frametime % 1000000 )) ) 22 | nframenum="$( printf "%08d" $framenum )" 23 | ifilename="${avifile}.tmp_${filenum}.tiff" 24 | ofilename="frame_${nframenum}.time_${secs}.${usecs}.tiff" 25 | mv "$ifilename" "$ofilename" 26 | done 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/extract_images.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | avifile="$1" 4 | timesfile="${1}.times" 5 | 6 | if [ ! \( -e "$avifile" -a -e "$timesfile" \) ]; then 7 | echo "Usage: $0 capture_.xxxxxx.avi" 8 | echo "will extract images from avi file with timestamp encoded in filename" 9 | exit 10 | fi 11 | 12 | echo extracting images from avi 13 | ffmpeg -i "$avifile" -vsync 0 -start_number 0 "${avifile}.tmp_%08d.tiff" 14 | 15 | echo "renaming frames" 16 | count=0; 17 | cat $timesfile | while read framenum frametime; do 18 | filenum="$( printf "%08d" $(( count )) )" 19 | count=$(( count + 1 )) 20 | secs=$(( frametime/1000000 )) 21 | usecs=$( printf "%06d" $(( frametime % 1000000 )) ) 22 | nframenum="$( printf "%08d" $framenum )" 23 | ifilename="${avifile}.tmp_${filenum}.tiff" 24 | ofilename="frame_${nframenum}.time_${secs}.${usecs}.tiff" 25 | mv "$ifilename" "$ofilename" 26 | done 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /packages/flight/diff_gps/src/serialReadlinefunctionCode: -------------------------------------------------------------------------------- 1 | Serial::readline (string &buffer, size_t size, string eol) 2 | { 3 | ScopedReadLock lock(this->pimpl_); 4 | size_t eol_len = eol.length (); 5 | uint8_t *buffer_ = static_cast 6 | (alloca (size * sizeof (uint8_t))); 7 | size_t read_so_far = 0; 8 | while (true) 9 | { 10 | size_t bytes_read = this->read_ (buffer_ + read_so_far, 1); 11 | read_so_far += bytes_read; 12 | if (bytes_read == 0) { 13 | break; // Timeout occured on reading 1 byte 14 | } 15 | if (string (reinterpret_cast 16 | (buffer_ + read_so_far - eol_len), eol_len) == eol) { 17 | break; // EOL found 18 | } 19 | if (read_so_far == size) { 20 | break; // Reached the maximum read length 21 | } 22 | } 23 | buffer.append(reinterpret_cast (buffer_), read_so_far); 24 | return read_so_far; 25 | } 26 | -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/include/model_distance_from_height/models/Model2D.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 10.05.17. 3 | // 4 | 5 | #ifndef MODEL_DISTANCE_FROM_HEIGHT_MODEL2D_H 6 | #define MODEL_DISTANCE_FROM_HEIGHT_MODEL2D_H 7 | 8 | #include "ModelBase.h" 9 | 10 | namespace model_distance_from_height { 11 | 12 | class Model2D : public ModelBase { 13 | /// Implements a basic 2d model for computing the distance to an object, given its height 14 | /// Works fully on the Y-Z camera plane 15 | 16 | //TODO: Not finished! 17 | 18 | protected: 19 | 20 | struct height_model_simple height_model_; 21 | 22 | double fy_; // focal length in pixels 23 | double cy_; // camera central point 24 | 25 | public: 26 | Model2D(){}; 27 | Model2D(int fy, int cy, double hmean); 28 | 29 | double compute_distance(double pitch, double height_pixels, double center_pixels); 30 | }; 31 | } 32 | 33 | 34 | #endif //MODEL_DISTANCE_FROM_HEIGHT_MODEL2D_H 35 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/launch/publish_target.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/src/random_walk.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from geometry_msgs.msg import Twist 4 | import scipy.io as sio 5 | 6 | temp = sio.loadmat('/is/ps2/rtallamraju/mpi_experimental_rahul/src/Packages/simulation/random_moving_target/src/command_vels.mat') 7 | v = temp['vel_array'] 8 | w = temp['omega_array'] 9 | 10 | def commands(): 11 | counter = 0 12 | val = 0 13 | rospy.init_node('random_walk',anonymous=True) 14 | pub=rospy.Publisher('/target_1/cmd_vel', Twist, queue_size=10) 15 | vel_msg = Twist() 16 | rate = rospy.Rate(1) # 10hz 17 | 18 | while not rospy.is_shutdown(): 19 | vel_msg.linear.x = v[counter][0] 20 | vel_msg.angular.z = w[counter][0] 21 | vel_msg.linear.y = 0 22 | vel_msg.linear.z = 0 23 | vel_msg.angular.x = 0 24 | vel_msg.angular.y = 0 25 | pub.publish(vel_msg) 26 | counter = counter + 1; 27 | rate.sleep() 28 | 29 | 30 | if __name__ == '__main__': 31 | try: 32 | commands() 33 | except rospy.ROSInterruptException: pass -------------------------------------------------------------------------------- /packages/simulation/aircap/launch/recorded_overlay.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 | -------------------------------------------------------------------------------- /packages/flight/uav_msgs/msg/uav_pose.msg: -------------------------------------------------------------------------------- 1 | # A representation of 3D position, 3D orientation and 3D velocity and in free space, composed of position and orientation. 2 | # Point position (north,east,down) 3 | # Point velocity (north,east,down) 4 | # Quaternion orientation (x,y,z,w -- 0,0,0,1 = X(front)-> north, Y(right)-> east, Z(bottom)) 5 | # float64[100] covariance diagonal 10x10 matrix, column order: posN,posE,posD,velN,velE,velD,Qx,Qy,Qz,Qw 6 | # Point rotation (roll,pitch,yaw) 7 | # float64 thrust (power setting -1 <= thrust <= +1 , negative thrust=engine off) 8 | # int32 flightmode (TODO: to be defined later) 9 | # NOTE: To include complete covariance information, a float32[100] Covariance (10x10 matrix) would be needed as well. Transferring that with every update might limit bandwidth significantly. 10 | 11 | 12 | Header header 13 | geometry_msgs/Point position 14 | geometry_msgs/Point velocity 15 | geometry_msgs/Quaternion orientation 16 | float64[100] covariance 17 | geometry_msgs/Point angVelocity 18 | float64 thrust 19 | int32 flightmode 20 | geometry_msgs/Point POI -------------------------------------------------------------------------------- /scrips/simulation/do_experiments.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | EXP=$1 4 | 5 | if [ ! -e "$EXP" ]; then 6 | echo "Usage: $0 " 7 | exit 1 8 | fi 9 | 10 | echo Conducting experiment $EXP.... 11 | 12 | i=0 13 | for line in $( cat $EXP|sed -e 's/\t/X/' -e 's/\t/Y/' ); do 14 | robots=$( echo $line |sed -e 's/X.*//' ) 15 | comperc=$( echo $line |sed -e 's/.*X//' -e 's/Y.*//' ) 16 | runs=$( echo $line |sed -e 's/.*Y//' ) 17 | 18 | run=0 19 | while [ $(( run < runs )) = 1 ]; do 20 | i=$(( i+1 )) 21 | run=$(( run+1 )) 22 | 23 | echo $i run $run: $robots $comperc $EXP 24 | 25 | result=1; 26 | failures=0; 27 | while [ $result != 0 -a $failures -lt 5 ]; do 28 | timeout 2400 ./experiment_mavocap_gazebo.sh $robots $comperc ${EXP}_${i}_${robots}_${comperc}_${run} 29 | result=$? 30 | if [ $result != 0 ]; then 31 | failures=$(( failures+1 )) 32 | ./cleanup.sh 33 | sleep 10 34 | fi 35 | done 36 | if [ $result != 0 ]; then 37 | echo "Experiment ${EXP}_${i}_${robots}_${comperc}_${run} failed!!!!" 38 | fi 39 | done 40 | done 41 | 42 | 43 | -------------------------------------------------------------------------------- /packages/optional/gcs_visualization/src/gcs_visualization/world.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import numpy 3 | import tf #transformations 4 | import tf2_ros 5 | from geometry_msgs.msg import TransformStamped 6 | 7 | WORLD_FRAME = 'world' 8 | RVIZ_FRAME = 'world_ENU' 9 | 10 | class WorldToRVIZ(): 11 | 12 | def __init__(self): 13 | 14 | self.br = tf2_ros.StaticTransformBroadcaster() 15 | 16 | static_transformStamped = TransformStamped() 17 | static_transformStamped.header.stamp = rospy.Time.now() 18 | static_transformStamped.header.frame_id = RVIZ_FRAME 19 | static_transformStamped.child_frame_id = WORLD_FRAME 20 | 21 | quat = tf.transformations.quaternion_from_euler(numpy.pi, 0, numpy.pi/2.0) 22 | static_transformStamped.transform.rotation.x = quat[0] 23 | static_transformStamped.transform.rotation.y = quat[1] 24 | static_transformStamped.transform.rotation.z = quat[2] 25 | static_transformStamped.transform.rotation.w = quat[3] 26 | 27 | self.br.sendTransform(static_transformStamped) -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/package.xml: -------------------------------------------------------------------------------- 1 | 2 | generic_potential_field 3 | 1.0.0 4 | generic_potential_field is ported from telekyb_calculus contains calculus elements like artifical potential functions... 5 | Aamir Ahmad 6 | 7 | GPLv3 8 | 9 | 10 | Martin Riedel (original author), Aamir Ahmad (current). 11 | 12 | 13 | catkin 14 | 15 | 16 | roscpp 17 | cmake_modules 18 | dynamic_reconfigure 19 | uav_msgs 20 | 21 | roscpp 22 | dynamic_reconfigure 23 | uav_msgs 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/src/target_robot.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include "geometry_msgs/Twist.h" 4 | #include "std_msgs/String.h" 5 | 6 | 7 | /** 8 | * This tutorial demonstrates simple sending of velocity commands to the IRobot Create in Gazebo. 9 | */ 10 | int main(int argc, char **argv) 11 | { 12 | 13 | ros::init(argc, argv, "CreateController"); 14 | 15 | ros::NodeHandle n; 16 | 17 | ros::Publisher vel_pub_0 = n.advertise("/target_1/cmd_vel", 1); 18 | // ros::Publisher vel_pub_1 = n.advertise("/robot0/cmd_vel", 1); 19 | ros::Rate loop_rate(5); 20 | 21 | int count = 0; 22 | 23 | while (ros::ok()) 24 | { 25 | geometry_msgs::Twist cmd_vel; 26 | 27 | cmd_vel.linear.x = 0.5; 28 | cmd_vel.linear.y = 0; 29 | cmd_vel.linear.z = 0; 30 | cmd_vel.angular.x = 0; 31 | cmd_vel.angular.y = 0; 32 | cmd_vel.angular.z = 0.4*sin(count); 33 | 34 | 35 | vel_pub_0.publish(cmd_vel); 36 | ros::spinOnce(); 37 | 38 | loop_rate.sleep(); 39 | 40 | ++count; 41 | } 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /packages/flight/target_tracker_distributed_kf/cfg/KalmanFilterParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "target_tracker_distributed_kf" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("noisePosXVar", double_t, 0, "noisePosXVar", 0.5, 0, 5) 9 | gen.add("noisePosYVar", double_t, 0, "noisePosYVar", 0.5, 0, 5) 10 | gen.add("noisePosZVar", double_t, 0, "noisePosZVar", 0.4, 0, 5) 11 | 12 | 13 | gen.add("noiseVelXVar", double_t, 0, "noiseVelXVar", 0.3, 0, 20) 14 | gen.add("noiseVelYVar", double_t, 0, "noiseVelYVar", 0.3, 0, 20) 15 | gen.add("noiseVelZVar", double_t, 0, "noiseVelZVar", 0.3, 0, 20) 16 | 17 | gen.add("noiseOffXVar", double_t, 0, "noiseOffXVar", 0.3, 0, 20) 18 | gen.add("noiseOffYVar", double_t, 0, "noiseOffYVar", 0.3, 0, 20) 19 | gen.add("noiseOffZVar", double_t, 0, "noiseOffZVar", 0.3, 0, 20) 20 | 21 | gen.add("velocityDecayTime", double_t, 0, "velocityDecayTime", 3.0, 0.0, 10.0) 22 | gen.add("offsetDecayTime", double_t, 0, "offsetDecayTime", 30.0, 0.0, 300.0) 23 | 24 | exit(gen.generate(PACKAGE, "target_tracker", "KalmanFilterParams")) 25 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/src/move_robot_randomly.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include "geometry_msgs/Twist.h" 4 | #include "std_msgs/String.h" 5 | 6 | 7 | /** 8 | * This tutorial demonstrates simple sending of velocity commands to the IRobot Create in Gazebo. 9 | */ 10 | int main(int argc, char **argv) 11 | { 12 | 13 | ros::init(argc, argv, "CreateController"); 14 | 15 | ros::NodeHandle n; 16 | 17 | ros::Publisher vel_pub_0 = n.advertise("/target_robot/cmd_vel", 1); 18 | // ros::Publisher vel_pub_1 = n.advertise("/robot0/cmd_vel", 1); 19 | ros::Rate loop_rate(5); 20 | 21 | int count = 0; 22 | 23 | while (ros::ok()) 24 | { 25 | geometry_msgs::Twist cmd_vel; 26 | 27 | cmd_vel.linear.x = 0.5; 28 | cmd_vel.linear.y = 0; 29 | cmd_vel.linear.z = 0; 30 | cmd_vel.angular.x = 0; 31 | cmd_vel.angular.y = 0; 32 | cmd_vel.angular.z = 0.4*sin(count); 33 | 34 | 35 | vel_pub_0.publish(cmd_vel); 36 | ros::spinOnce(); 37 | 38 | loop_rate.sleep(); 39 | 40 | ++count; 41 | } 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/include/model_distance_from_height/models/Model3D.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 10.05.17. 3 | // 4 | 5 | #ifndef MODEL_DISTANCE_FROM_HEIGHT_MODEL3D_H 6 | #define MODEL_DISTANCE_FROM_HEIGHT_MODEL3D_H 7 | 8 | #include "ModelBase.h" 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace model_distance_from_height { 15 | 16 | class Model3D : public ModelBase { 17 | /// Implements the 3d model for computing the distance to an object, given its height and center 18 | 19 | public: 20 | struct height_model_uncertainty height_model_; 21 | 22 | Model3D(){}; 23 | Model3D(double hmean, double hvar); 24 | 25 | double compute_distance(const geometry_msgs::Point& p, double ymin, double delta_y); 26 | double compute_dist_var(const geometry_msgs::PoseWithCovariance& pose, const double delta_y, const double var_delta_y, const double ymin, const double var_ymin, const double dist); 27 | }; 28 | } 29 | 30 | 31 | #endif //MODEL_DISTANCE_FROM_HEIGHT_MODEL3D_H 32 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/launch/move_target_withID.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 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/src/CameraInfoPublisher.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Provides a sensor_msgs/CameraInfo message in a latched topic 3 | // Info is read from URL using the camera_info_manager interface 4 | // 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char* argv[]){ 10 | 11 | ros::init(argc, argv, "camera_info_publisher"); 12 | 13 | ros::NodeHandle nh; 14 | ros::NodeHandle pnh("~"); 15 | 16 | // Parameters 17 | std::string url; 18 | if(!pnh.getParam("camera_info_url", url)){ 19 | ROS_ERROR("Must provide camera_info_url parameter"); 20 | return EXIT_FAILURE; 21 | } 22 | camera_info_manager::CameraInfoManager manager(nh, "video", url); 23 | 24 | std::string topic{"camera_info"}; 25 | pnh.getParam("camera_info_topic", topic); 26 | 27 | ros::Publisher pub = nh.advertise(topic, 1, true); 28 | 29 | ROS_INFO("Publishing camera_info to topic %s", pub.getTopic().c_str()); 30 | 31 | // Publish once, then just spin because it's a latched topic 32 | pub.publish(manager.getCameraInfo()); 33 | 34 | ros::spin(); 35 | 36 | return EXIT_SUCCESS; 37 | } 38 | -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/include/model_distance_from_height/models/ModelBase.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 10.05.17. 3 | // 4 | 5 | #ifndef MODEL_DISTANCE_FROM_HEIGHT_MODELBASE_H 6 | #define MODEL_DISTANCE_FROM_HEIGHT_MODELBASE_H 7 | 8 | namespace model_distance_from_height { 9 | 10 | struct height_model_simple{ 11 | double mean; 12 | 13 | height_model_simple(double _mean) : mean(_mean){} 14 | height_model_simple() {}; 15 | }; 16 | 17 | struct height_model_uncertainty{ 18 | double mean, var; 19 | 20 | height_model_uncertainty(double _mean, double _var) : mean(_mean), var(_var){}; 21 | height_model_uncertainty() {}; 22 | }; 23 | 24 | class ModelBase { 25 | /// Every model will provide the distance to an object on the image given certain information about it 26 | /// Example of information is: height model, camera angle, detected height in pixels, object center in pixels 27 | /// Camera calibration should not be handled inside a Model object, but outside 28 | protected: 29 | 30 | public: 31 | //virtual double compute_distance() = 0; 32 | //virtual double compute_uncertainty() = 0; 33 | }; 34 | } 35 | 36 | #endif //MODEL_DISTANCE_FROM_HEIGHT_MODELBASE_H -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/src/libavhelper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * libavformat API example. 4 | * 5 | * Output a media file in any supported libavformat format. The default 6 | * codecs are used. 7 | * @example muxing.c 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | int libavhelper_init_write(const char *filename, const char* format, const char* codec, int width, int height, int framerate, enum AVPixelFormat pixelformatout, unsigned int threads); 25 | int libavhelper_init_read(const char *filename, int* width, int *height, enum AVPixelFormat *pixelformatin, unsigned int threads); 26 | void libavhelper_close_read(void); 27 | void libavhelper_close_write(void); 28 | void libavhelper_save_frame(uint8_t* buffer, unsigned long long timestamp); 29 | int libavhelper_read_frame(uint8_t* buffer, unsigned long long *timestamp); 30 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/src/libavhelper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * libavformat API example. 4 | * 5 | * Output a media file in any supported libavformat format. The default 6 | * codecs are used. 7 | * @example muxing.c 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | int libavhelper_init_write(const char *filename, const char* format, const char* codec, int width, int height, int framerate, enum AVPixelFormat pixelformatout, unsigned int threads); 25 | int libavhelper_init_read(const char *filename, int* width, int *height, enum AVPixelFormat *pixelformatin, unsigned int threads); 26 | void libavhelper_close_read(void); 27 | void libavhelper_close_write(void); 28 | void libavhelper_save_frame(uint8_t* buffer, unsigned long long timestamp); 29 | int libavhelper_read_frame(uint8_t* buffer, unsigned long long *timestamp); 30 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/src/libavhelper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * libavformat API example. 4 | * 5 | * Output a media file in any supported libavformat format. The default 6 | * codecs are used. 7 | * @example muxing.c 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | int libavhelper_init_write(const char *filename, const char* format, const char* codec, int width, int height, int framerate, enum AVPixelFormat pixelformatout, unsigned int threads); 25 | int libavhelper_init_read(const char *filename, int* width, int *height, enum AVPixelFormat *pixelformatin, unsigned int threads); 26 | void libavhelper_close_read(void); 27 | void libavhelper_close_write(void); 28 | void libavhelper_save_frame(uint8_t* buffer, unsigned long long timestamp); 29 | int libavhelper_read_frame(uint8_t* buffer, unsigned long long *timestamp); 30 | -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/src/models/Model3D.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 10.05.17. 3 | // 4 | 5 | #include 6 | 7 | namespace model_distance_from_height{ 8 | 9 | Model3D::Model3D(double hmean, double hvar) : height_model_(hmean, hvar) { 10 | ROS_ASSERT_MSG(hmean != 0, "The height model mean can't be 0"); 11 | } 12 | 13 | double Model3D::compute_distance(const geometry_msgs::Point& p, double ymin, double delta_y) { 14 | //z == 1/2*(p3*real_person_height*ymin - p2*real_person_height)/(ycenter - ymin) 15 | //z = H*(p3*ymin - p2)/(height_pixels) 16 | //where height_pixels = ymax-ymin 17 | 18 | ROS_ASSERT(delta_y != 0.0); 19 | return height_model_.mean * (p.z * ymin - p.y) / delta_y; 20 | } 21 | 22 | double 23 | Model3D::compute_dist_var(const geometry_msgs::PoseWithCovariance &pose, const double delta_y, const double var_delta_y, 24 | const double ymin, const double var_ymin, const double dist) { 25 | 26 | //TODO update this simplified model 27 | ROS_ASSERT(delta_y != 0.0); 28 | return dist*dist*(height_model_.var/height_model_.mean + var_delta_y/delta_y); 29 | } 30 | 31 | 32 | } -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver_baseMPC/OCPsolver.h: -------------------------------------------------------------------------------- 1 | #ifndef OCPSOLVER_H 2 | #define OCPSOLVER_H 3 | 4 | #include 5 | 6 | extern "C"{ 7 | #include "solver.h" 8 | } 9 | 10 | 11 | 12 | class OCPSolver 13 | { 14 | 15 | public: 16 | OCPSolver(); 17 | 18 | // Design a trajectory containing desired position, velocity, acceleration, jerk via the solution of the convex optimization problem 19 | Eigen::Matrix3d OCPsolDesigner(double deltaT, Eigen::Vector3d ExternalForce, Eigen::Vector3d CurPosition, Eigen::Vector3d CurVelocity, Eigen::Matrix ReferencePath, Eigen::Vector3d Waypoint, Eigen::Vector3d desVelocity, Eigen::Matrix3d CostFunction); 20 | // Load and generate required parameters for the optimal control problem 21 | void load_data(double deltaT, Eigen::Vector3d ExternalForce, Eigen::Matrix InitialState, Eigen::Matrix TerminalState, Eigen::Matrix ReferencePath, Eigen::Matrix StateConstraint, Eigen::Matrix InputConstraint, Eigen::Matrix3d CostFunction); 22 | // output the solution of decoupled optimal control problems 23 | Eigen::Matrix3d use_solution(Vars vars); 24 | 25 | }; 26 | 27 | #endif // OCPSOLVER_H 28 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/src/TestCalculus.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TestCalculus.cpp 3 | * 4 | * Created on: Sep 8, 2012 5 | * Author: mriedel 6 | */ 7 | 8 | 9 | #include <../include/CoTanPotentialFunctions.hpp> 10 | 11 | #define LOWER_BOUND 1.0 12 | #define UPPER_BOUND 7.0 13 | 14 | int main(int argc, char **argv) 15 | { 16 | ros::init(argc,argv,"TestCalculus"); 17 | 18 | 19 | CoTanRepulsiveGradient funcRep("RepGradient", 2,6,10,1); 20 | CoTanAttractiveGradient funcAttr("AttrGradient", 2,6,10,1); 21 | 22 | // PotentialFunction testGradient("testGradient", 23 | // 6, 2, 10, 1); 24 | 25 | // PotentialFunction testHassian("testHassian", 26 | // 6, 2, 10, 1); 27 | 28 | double d = LOWER_BOUND; 29 | while(ros::ok()) { 30 | 31 | ROS_INFO("Value: %f, RepGradient: %f",d, funcRep.getPotential(d,2)); 32 | ROS_INFO("Value: %f, AttGradient: %f",d, funcAttr.getPotential(d,2)); 33 | 34 | ROS_INFO("Value: %f, sat value: %f",d, funcRep.tPotFuncSatValue); 35 | ROS_INFO("Value: %f, gain value: %f",d, funcAttr.tPotFuncGain); 36 | 37 | d += 0.1; 38 | if (d > UPPER_BOUND) { 39 | d = LOWER_BOUND; 40 | } 41 | 42 | 43 | 44 | usleep(1000*100); 45 | } 46 | 47 | 48 | ros::shutdown(); 49 | return 0; 50 | } 51 | 52 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver/MPCsolver.h: -------------------------------------------------------------------------------- 1 | #ifndef OCPSOLVER_H 2 | #define OCPSOLVER_H 3 | 4 | #include 5 | 6 | extern "C"{ 7 | #include "solver.h" 8 | } 9 | 10 | 11 | 12 | class MPCsolver 13 | { 14 | 15 | public: 16 | MPCsolver(); 17 | 18 | // Design a trajectory containing desired position, velocity, acceleration, jerk via the solution of the convex optimization problem 19 | Eigen::Matrix3d OCPsolDesigner(double deltaT, Eigen::Vector3d ExternalForce, Eigen::Matrix curState, Eigen::Matrix ReferencePath, Eigen::Matrix termState, Eigen::Matrix CostFunction, Eigen::Matrix stateLimits, Eigen::Matrix input_constraint); 20 | 21 | // Load and generate required parameters for the optimal control problem 22 | void load_data(double deltaT, Eigen::Vector3d ExternalForce, Eigen::Matrix InitialState, Eigen::Matrix TerminalState, Eigen::Matrix ReferencePath, Eigen::Matrix StateConstraint, Eigen::Matrix InputConstraint, Eigen::Matrix CostFunction); 23 | 24 | // output the solution of decoupled optimal control problems 25 | Eigen::Matrix3d use_solution(Vars vars); 26 | 27 | }; 28 | 29 | #endif // OCPSOLVER_H 30 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(random_moving_target) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | ) 11 | 12 | # find_package( OpenCV REQUIRED ) 13 | # include_directories(SYSTEM ${OpenCV_INCLUDE_DIRS}) 14 | find_package(catkin REQUIRED sensor_msgs cv_bridge roscpp std_msgs image_transport) 15 | find_package(Boost REQUIRED COMPONENTS system) 16 | include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 17 | 18 | 19 | catkin_package( 20 | # INCLUDE_DIRS include 21 | # LIBRARIES random_moving_target 22 | # CATKIN_DEPENDS roscpp rospy 23 | # DEPENDS system_lib 24 | ) 25 | 26 | 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | 31 | add_executable(virtual_target_square_motion src/virtual_target_square_motion.cpp) 32 | target_link_libraries(virtual_target_square_motion ${catkin_LIBRARIES}) 33 | 34 | 35 | add_executable(target_robot src/target_robot.cpp) 36 | target_link_libraries(target_robot ${catkin_LIBRARIES}) 37 | 38 | add_executable(publish_target_robot_state src/publish_target_robot_state.cpp) 39 | target_link_libraries(publish_target_robot_state ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /packages/flight/camera_configs/launch/video_stream_ptgrey.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/launch/spawn_move_target_withID.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 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver_act/MPCsolver.h: -------------------------------------------------------------------------------- 1 | #ifndef OCPSOLVER_H 2 | #define OCPSOLVER_H 3 | 4 | #include 5 | 6 | extern "C"{ 7 | #include "solver.h" 8 | } 9 | 10 | 11 | 12 | class MPCsolver 13 | { 14 | 15 | public: 16 | MPCsolver(); 17 | 18 | // Design a trajectory containing desired position, velocity, acceleration, jerk via the solution of the convex optimization problem 19 | Eigen::Matrix OCPsolDesigner(double deltaT, Eigen::Matrix obstacle_force, Eigen::Vector3d ExternalForce, Eigen::Matrix curState, Eigen::Matrix termState, Eigen::Matrix CostFunction, Eigen::Matrix stateLimits, Eigen::Matrix input_constraint); 20 | 21 | // Load and generate required parameters for the optimal control problem 22 | void load_data(double deltaT, Eigen::Matrix obstacle_force, Eigen::Vector3d ExternalForce, Eigen::Matrix InitialState, Eigen::Matrix TerminalState, Eigen::Matrix StateConstraint, Eigen::Matrix InputConstraint, Eigen::Matrix CostFunction); 23 | 24 | // output the solution of decoupled optimal control problems 25 | // x vx, y,vy,z,vz for 16 time steps and one more field for latest control inputs. 26 | Eigen::Matrix use_solution(Vars vars); 27 | 28 | }; 29 | 30 | #endif // OCPSOLVER_H 31 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver_obstacles_SSRR/MPCsolver.h: -------------------------------------------------------------------------------- 1 | #ifndef OCPSOLVER_H 2 | #define OCPSOLVER_H 3 | 4 | #include 5 | 6 | extern "C"{ 7 | #include "solver.h" 8 | } 9 | 10 | 11 | 12 | class MPCsolver 13 | { 14 | 15 | public: 16 | MPCsolver(); 17 | 18 | // Design a trajectory containing desired position, velocity, acceleration, jerk via the solution of the convex optimization problem 19 | Eigen::Matrix OCPsolDesigner(double deltaT, Eigen::Matrix obstacle_force, Eigen::Vector3d ExternalForce, Eigen::Matrix curState, Eigen::Matrix termState, Eigen::Matrix CostFunction, Eigen::Matrix stateLimits, Eigen::Matrix input_constraint); 20 | 21 | // Load and generate required parameters for the optimal control problem 22 | void load_data(double deltaT, Eigen::Matrix obstacle_force, Eigen::Vector3d ExternalForce, Eigen::Matrix InitialState, Eigen::Matrix TerminalState, Eigen::Matrix StateConstraint, Eigen::Matrix InputConstraint, Eigen::Matrix CostFunction); 23 | 24 | // output the solution of decoupled optimal control problems 25 | // x vx, y,vy,z,vz for 16 time steps and one more field for latest control inputs. 26 | Eigen::Matrix use_solution(Vars vars); 27 | 28 | }; 29 | 30 | #endif // OCPSOLVER_H 31 | -------------------------------------------------------------------------------- /packages/optional/gcs_visualization/.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | #Pycharm 10 | .idea/ 11 | 12 | # Distribution / packaging 13 | .Python 14 | env/ 15 | build/ 16 | develop-eggs/ 17 | dist/ 18 | downloads/ 19 | eggs/ 20 | .eggs/ 21 | lib/ 22 | lib64/ 23 | parts/ 24 | sdist/ 25 | var/ 26 | *.egg-info/ 27 | .installed.cfg 28 | *.egg 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *,cover 49 | .hypothesis/ 50 | 51 | # Translations 52 | *.mo 53 | *.pot 54 | 55 | # Django stuff: 56 | *.log 57 | local_settings.py 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # IPython Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # dotenv 82 | .env 83 | 84 | # virtualenv 85 | venv/ 86 | ENV/ 87 | 88 | # Spyder project settings 89 | .spyderproject 90 | 91 | # Rope project settings 92 | .ropeproject 93 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver_obstacles/MPCsolver.h: -------------------------------------------------------------------------------- 1 | #ifndef OCPSOLVER_H 2 | #define OCPSOLVER_H 3 | 4 | #include 5 | 6 | extern "C"{ 7 | #include "solver.h" 8 | } 9 | 10 | 11 | 12 | class MPCsolver 13 | { 14 | 15 | public: 16 | MPCsolver(); 17 | 18 | // Design a trajectory containing desired position, velocity, acceleration, jerk via the solution of the convex optimization problem 19 | Eigen::Matrix OCPsolDesigner(double deltaT, Eigen::Matrix obstacle_force, Eigen::Vector3d ExternalForce, Eigen::Matrix curState, Eigen::Matrix ReferencePath, Eigen::Matrix termState, Eigen::Matrix CostFunction, Eigen::Matrix stateLimits, Eigen::Matrix input_constraint); 20 | 21 | // Load and generate required parameters for the optimal control problem 22 | void load_data(double deltaT, Eigen::Matrix obstacle_force, Eigen::Vector3d ExternalForce, Eigen::Matrix InitialState, Eigen::Matrix TerminalState, Eigen::Matrix ReferencePath, Eigen::Matrix StateConstraint, Eigen::Matrix InputConstraint, Eigen::Matrix CostFunction); 23 | 24 | // output the solution of decoupled optimal control problems 25 | // x vx, y,vy,z,vz for 16 time steps and one more field for latest control inputs. 26 | Eigen::Matrix use_solution(Vars vars); 27 | 28 | }; 29 | 30 | #endif // OCPSOLVER_H 31 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/include/CoTanPotentialFunctions.hpp: -------------------------------------------------------------------------------- 1 | #ifndef COTANPOTENTIALFUNCTIONS_HPP_ 2 | #define COTANPOTENTIALFUNCTIONS_HPP_ 3 | 4 | 5 | #include 6 | // #include 7 | // #include 8 | 9 | 10 | class CoTanRepulsiveGradient : public PotentialFunction { 11 | 12 | // private: 13 | // dynamic_reconfigure::Server dynamicServer_; 14 | 15 | public: 16 | CoTanRepulsiveGradient(const std::string& potentialFunctionName_); 17 | CoTanRepulsiveGradient(const std::string& potentialFunctionName_, 18 | double tPotFuncZeroD_, double tPotFuncInfD_, double tPotFuncSatValue_, double tPotFuncGain_); 19 | 20 | double getPotentialImpl(double d, double threshold) const; 21 | // void reconf_callback(generic_potential_field::potentialParamsConfig&); 22 | }; 23 | 24 | 25 | class CoTanAttractiveGradient : public PotentialFunction { 26 | 27 | // private: 28 | // dynamic_reconfigure::Server dynamicServer_; 29 | 30 | public: 31 | CoTanAttractiveGradient(const std::string& potentialFunctionName_); 32 | CoTanAttractiveGradient(const std::string& potentialFunctionName_, 33 | double tPotFuncZeroD_, double tPotFuncInfD_, double tPotFuncSatValue_, double tPotFuncGain_); 34 | 35 | double getPotentialImpl(double d, double threshold) const; 36 | // void reconf_callback(generic_potential_field::potentialParamsConfig&); 37 | }; 38 | 39 | #endif /* COTANPOTENTIALFUNCTIONS_HPP_ */ 40 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver_act/description.cvxgen: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2018-04-10 14:30:30 -0400. 2 | # CVXGEN is Copyright (C) 2006-2017 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2017 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: description.cvxgen. 8 | # Description: A description of the CVXGEN problem. 9 | 10 | dimensions 11 | m = 3 # inputs 12 | n = 6 # states 13 | T = 15 # receding horizon 14 | end 15 | 16 | parameters 17 | A (n,n) {1,1 1,2 2,2 3,3 3,4 4,4 5,5 5,6 6,6 }# dynamics matrix 18 | B (n,m) {1,1 2,1 3,2 4,2 5,3 6,3}# transfer matrix 19 | Q (n,n) diagonal psd # terminal state cost 20 | R (m,m) diagonal psd # stage input cost 21 | x[0] (n) # initial state condition 22 | xN (n) # terminal state condition 23 | x_max (n) # state (position and velocity) upper bound 24 | x_min (n) # state (position and velocity) lower bound 25 | u_max (m) # input upper bound 26 | u_min (m) # input lower bound 27 | g (m) # affine term to describe gravity 28 | f_obs[t] (m), t=0..T # obstacle's potential 29 | end 30 | 31 | variables 32 | x[t] (n), t=1..T+1 # state 33 | u[t] (m), t=0..T # input 34 | end 35 | 36 | minimize 37 | sum[t=0..T](quad(u[t] + g + f_obs[t], R)) + quad(x[T+1]-xN, Q) 38 | subject to 39 | x[t+1] == A*x[t] + B*(u[t] + g + f_obs[t]), t=0..T # dynamics constraints 40 | x_min <= x[t] <= x_max, t=1..T+1 # state constraint 41 | u_min <= u[t] <= u_max, t=0..T # input constraint 42 | end 43 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver_obstacles_SSRR/description.cvxgen: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2018-04-10 14:30:30 -0400. 2 | # CVXGEN is Copyright (C) 2006-2017 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2017 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: description.cvxgen. 8 | # Description: A description of the CVXGEN problem. 9 | 10 | dimensions 11 | m = 3 # inputs 12 | n = 6 # states 13 | T = 15 # receding horizon 14 | end 15 | 16 | parameters 17 | A (n,n) {1,1 1,2 2,2 3,3 3,4 4,4 5,5 5,6 6,6 }# dynamics matrix 18 | B (n,m) {1,1 2,1 3,2 4,2 5,3 6,3}# transfer matrix 19 | Q (n,n) diagonal psd # terminal state cost 20 | R (m,m) diagonal psd # stage input cost 21 | x[0] (n) # initial state condition 22 | xN (n) # terminal state condition 23 | x_max (n) # state (position and velocity) upper bound 24 | x_min (n) # state (position and velocity) lower bound 25 | u_max (m) # input upper bound 26 | u_min (m) # input lower bound 27 | g (m) # affine term to describe gravity 28 | f_obs[t] (m), t=0..T # obstacle's potential 29 | end 30 | 31 | variables 32 | x[t] (n), t=1..T+1 # state 33 | u[t] (m), t=0..T # input 34 | end 35 | 36 | minimize 37 | sum[t=0..T](quad(u[t] + g + f_obs[t], R)) + quad(x[T+1]-xN, Q) 38 | subject to 39 | x[t+1] == A*x[t] + B*(u[t] + g + f_obs[t]), t=0..T # dynamics constraints 40 | x_min <= x[t] <= x_max, t=1..T+1 # state constraint 41 | u_min <= u[t] <= u_max, t=0..T # input constraint 42 | end 43 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/launch/overlay.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 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/launch/one_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(generic_potential_field) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS roscpp cmake_modules dynamic_reconfigure uav_msgs) 6 | find_package(Boost QUIET REQUIRED COMPONENTS system thread) 7 | find_package(Eigen REQUIRED) 8 | #set(EIGEN3_INCLUDE_DIRS $ENV{EIGEN3_INCLUDE_DIR}) 9 | #IF (NOT DEFINED EIGEN3_INCLUDE_DIRS) 10 | # MESSAGE(FATAL_ERROR "Please point to the direction of the eigen3 installation") 11 | #ENDIF() 12 | 13 | include(CheckCXXCompilerFlag) 14 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 15 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 16 | if(COMPILER_SUPPORTS_CXX11) 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 18 | elseif(COMPILER_SUPPORTS_CXX0X) 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 20 | else() 21 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 22 | endif() 23 | 24 | 25 | generate_dynamic_reconfigure_options( 26 | cfg/potentialParams.cfg 27 | ) 28 | 29 | INCLUDE_DIRECTORIES( 30 | ${PROJECT_SOURCE_DIR}/include 31 | ${PROJECT_SOURCE_DIR} 32 | ${EIGEN_INCLUDE_DIRS} 33 | ${catkin_INCLUDE_DIRS} 34 | ${Boost_INCLUDE_DIRS} 35 | ) 36 | 37 | 38 | add_library(${PROJECT_NAME} 39 | src/CoTanPotentialFunctions.cpp 40 | src/PotentialFunction.cpp 41 | ) 42 | 43 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 44 | 45 | add_executable(calculus_test_new src/TestCalculus.cpp) 46 | 47 | target_link_libraries(calculus_test_new 48 | ${Boost_LIBRARIES} 49 | ${catkin_LIBRARIES} 50 | ${PROJECT_NAME} 51 | ) 52 | 53 | catkin_package( 54 | DEPENDS roscpp Boost 55 | INCLUDE_DIRS include 56 | LIBRARIES ${PROJECT_NAME} 57 | ) 58 | 59 | -------------------------------------------------------------------------------- /packages/simulation/fake_communication_failure/bin/fake_communication_failure_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import random 4 | import math 5 | import rospy 6 | import message_filters 7 | import os 8 | #import cv2 9 | import tf 10 | import sys 11 | #from cv_bridge import CvBridge, CvBridgeError 12 | #from sensor_msgs.msg import Image, CameraInfo 13 | from roslib.message import get_message_class 14 | from rostopic import get_topic_class 15 | 16 | source=sys.argv[1] 17 | dest=sys.argv[2] 18 | percentage=(float)(sys.argv[3]) 19 | 20 | rospy.init_node("fake_communication_failure", log_level=rospy.INFO) 21 | 22 | 23 | rospy.loginfo("startup") 24 | messageClass=get_topic_class(source) 25 | rospy.loginfo("subscribing to class") 26 | rospy.loginfo(messageClass); 27 | 28 | if (messageClass[0]!=None): 29 | genericPublisher =rospy.Publisher(dest,messageClass[0],queue_size=10); 30 | else: 31 | rospy.loginfo("Deferring publisher until message type is known") 32 | gnericPublisher=0; 33 | 34 | 35 | def genericCallback(message): 36 | global percentage 37 | global messageClass 38 | global genericPublisher 39 | global dest 40 | if (messageClass[0]==None): 41 | messageClass=get_topic_class(source) 42 | rospy.loginfo("Late initialization of publisher, class") 43 | rospy.loginfo(messageClass); 44 | genericPublisher = rospy.Publisher(dest,messageClass[0],queue_size=10); 45 | #print("received a message") 46 | a=random.uniform(0.0,100.0) 47 | #print(a) 48 | #print(percentage) 49 | if (a<=percentage): 50 | #print("sending again") 51 | genericPublisher.publish(message) 52 | #else: 53 | # print("message discarded") 54 | 55 | genericSubscriber= rospy.Subscriber(source,rospy.msg.AnyMsg,genericCallback) 56 | 57 | rospy.loginfo("registered subscribers and publishers") 58 | rospy.loginfo("fake_communication_failure_node initialized") 59 | rospy.spin() 60 | 61 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/cfg/nmpcPlannerParams.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "nmpc_planner" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("neighborDistThreshold", double_t, 0, "neighborDistThreshold", 3.0, 1, 20) 9 | 10 | gen.add("approachAngleThreshold", double_t, 0, "approachAngleThreshold", 0.5, 0 , 20) 11 | 12 | gen.add("distanceThresholdToTarget", double_t, 0, "distanceThresholdToTarget", 8.0, 3, 15) 13 | 14 | gen.add("copterDesiredHeightinNED", double_t, 0, "copterDesiredHeightinNED", -8.0, -20.0, -2.0) 15 | 16 | gen.add("maxOffsetUncertaintyRadius", double_t, 0, "maxOffsetUncertaintyRadius", 3.0, 0 , 20) 17 | 18 | gen.add("targetGuaranteeThreshold", double_t, 0, "targetGuaranteeThreshold", 7.0, 0 , 20) 19 | 20 | gen.add("activeGuaranteeThreshold", double_t, 0, "activeGuaranteeThreshold", 1, 0 , 20) 21 | 22 | gen.add("obstacleGuaranteeThreshold", double_t, 0, "obstacleGuaranteeThreshold", 1.5, 0 , 20) 23 | 24 | gen.add("neighborGuaranteeThreshold", double_t, 0, "neighborGuaranteeThreshold", 1.5, 0 , 20) 25 | 26 | gen.add("INTERNAL_SUB_STEP", double_t, 0, "INTERNAL_SUB_STEP", 0.1, 0, 20) 27 | 28 | gen.add("deltaT", double_t, 0, "deltaT", 0.1, 0, 5.0) 29 | 30 | gen.add("activeTrackingWeight", double_t, 0, "activeTrackingWeight", 100, 0, 1000) 31 | gen.add("energyWeight", double_t, 0, "energyWeight", 10, 0, 1000) 32 | 33 | gen.add("copterVelocityLimitHorizontal", double_t, 0, "copterVelocityLimitHorizontal", 5, 0, 10.0) 34 | gen.add("copterVelocityLimitVertical", double_t, 0, "copterVelocityLimitVertical", 0.5, 0, 10.0) 35 | gen.add("copterAccelarationLimitHorizontal", double_t, 0, "copterAccelarationLimitHorizontal", 5, 0, 20.0) 36 | gen.add("copterAccelarationLimitVertical", double_t, 0, "copterAccelarationLimitVertical", 11, 0, 20.0) 37 | 38 | exit(gen.generate(PACKAGE, "nmpc_planner", "nmpcPlannerParams")) 39 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nmpc_planner 4 | 0.0.0 5 | The nmpc_planner package 6 | 7 | 8 | 9 | 10 | Aamir Ahmad 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | catkin 21 | geometry_msgs 22 | nav_msgs 23 | roscpp 24 | rospy 25 | std_msgs 26 | uav_msgs 27 | tf 28 | sensor_msgs 29 | mav_msgs 30 | generic_potential_field 31 | dynamic_reconfigure 32 | 33 | generic_potential_field 34 | mav_msgs 35 | sensor_msgs 36 | tf 37 | geometry_msgs 38 | nav_msgs 39 | roscpp 40 | rospy 41 | std_msgs 42 | uav_msgs 43 | dynamic_reconfigure 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/include/PotentialFunction.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POTENTIALFUNCTION_HPP_ 2 | #define POTENTIALFUNCTION_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include // C = kroneckerProduct(A,B); 30 | 31 | 32 | #include 33 | 34 | enum PotentialFunctionType { Attractive, Repulsive}; 35 | 36 | class PotentialFunction 37 | { 38 | 39 | protected: 40 | PotentialFunctionType type; 41 | 42 | void updateBoundsValues(); 43 | 44 | 45 | public: 46 | PotentialFunction(const std::string& potentialFunctionName_, PotentialFunctionType type_); 47 | 48 | PotentialFunction(const std::string& potentialFunctionName_, PotentialFunctionType type_, 49 | double tPotFuncZeroD_, double tPotFuncInfD_, double tPotFuncSatValue_, double tPotFuncGain_); 50 | 51 | 52 | 53 | // get Type 54 | PotentialFunctionType getType() const; 55 | 56 | double getPotential(double d, double threshold) const; 57 | // Pure virtual, overwrite 58 | virtual double getPotentialImpl(double d,double threshold) const = 0; 59 | 60 | double tPotFuncZeroD; 61 | double tPotFuncInfD; 62 | double tPotFuncSatValue; 63 | double tPotFuncGain; 64 | 65 | }; 66 | 67 | #endif /* POTENTIALFUNCTION_HPP_ */ 68 | -------------------------------------------------------------------------------- /packages/simulation/aircap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aircap 4 | 0.0.0 5 | The aircap package 6 | 7 | 8 | 9 | 10 | Eric Price 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/include/cv/extensions/projection.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 05.05.17. 3 | // 4 | 5 | #ifndef CV_PROJECTION_H 6 | #define CV_PROJECTION_H 7 | 8 | #include 9 | 10 | namespace cv{ 11 | 12 | template 13 | inline Point_ operator*(const Point_& lp, const Point_& rp){ 14 | return Point_(lp.x * rp.x, lp.y * rp.y); 15 | } 16 | template 17 | inline Point_ operator/(const Point_& lp, const Point_& rp){ 18 | return Point_(lp.x / rp.x, lp.y / rp.y); 19 | } 20 | 21 | template 22 | inline Point_ operator/(const T1& lp,const Point_& rp){ 23 | return Point_((T)lp / rp.x, (T)lp / rp.y); 24 | } 25 | 26 | template 27 | struct projection { 28 | TS scale; 29 | T offset; 30 | 31 | projection(const TS& _scale, const T& _offset) : scale(_scale), offset(_offset) {}; 32 | 33 | inline T forward(const T &in) const { return (T)((TS)in * scale) - offset; }; 34 | 35 | inline T backward(const T &in) const { return (T)((TS)(in + offset) * (1.0/scale)); }; 36 | 37 | inline T operator<<(const T &in) const { return backward(in); }; 38 | 39 | inline projection operator>>(const projection &pr) const { 40 | return projection(this->scale * pr.scale,(T)(pr.scale*(TS)this->offset) + pr.offset); 41 | }; 42 | 43 | inline projection operator<<(const projection &pr) const { 44 | return *this >> pr; 45 | }; 46 | 47 | }; 48 | 49 | // point = point >> proj 50 | template 51 | inline Point_ operator >>(const Point_ &pt, const projection &pr) { return pr.forward(pt); }; 52 | 53 | // alias for point2 << (proj << point1) 54 | template 55 | void operator<<(Point_ &pt,const Point_ &pt2) { pt=pt2; }; 56 | template 57 | void operator>>(const Point_ &pt,Point_ &pt2) { pt2=pt; }; 58 | 59 | 60 | 61 | typedef projection projection2i; 62 | } 63 | 64 | #endif //CV_PROJECTION_H 65 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/basler/orbslam_outdoors_basler.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 1487.006175 9 | Camera.fy: 1487.708688 10 | Camera.cx: 1022.805235 11 | Camera.cy: 593.765679 12 | 13 | Camera.k1: -0.103322 14 | Camera.k2: 0.190644 15 | Camera.p1: 0.000803 16 | Camera.p2: -0.001449 17 | 18 | Camera.width: 2040 19 | Camera.height: 1086 20 | 21 | # Camera frames per second 22 | Camera.fps: 60.0 23 | 24 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 25 | Camera.RGB: 0 26 | 27 | #-------------------------------------------------------------------------------------------- 28 | # ORB Parameters 29 | #-------------------------------------------------------------------------------------------- 30 | 31 | # ORB Extractor: Number of features per image 32 | ORBextractor.nFeatures: 1500 33 | 34 | # ORB Extractor: Scale factor between levels in the scale pyramid 35 | ORBextractor.scaleFactor: 1.2 36 | 37 | # ORB Extractor: Number of levels in the scale pyramid 38 | ORBextractor.nLevels: 8 39 | 40 | # ORB Extractor: Fast threshold 41 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 42 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 43 | # You can lower these values if your images have low contrast 44 | ORBextractor.iniThFAST: 20 45 | ORBextractor.minThFAST: 7 46 | 47 | #-------------------------------------------------------------------------------------------- 48 | # Viewer Parameters 49 | #-------------------------------------------------------------------------------------------- 50 | Viewer.KeyFrameSize: 0.05 51 | Viewer.KeyFrameLineWidth: 1 52 | Viewer.GraphLineWidth: 0.9 53 | Viewer.PointSize: 2 54 | Viewer.CameraSize: 0.08 55 | Viewer.CameraLineWidth: 3 56 | Viewer.ViewpointX: 0 57 | Viewer.ViewpointY: -0.7 58 | Viewer.ViewpointZ: -1.8 59 | Viewer.ViewpointF: 500 60 | 61 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_c920/orbslam_settings_c920.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 1414.33708 9 | Camera.fy: 1413.58507 10 | Camera.cx: 940.69096 11 | Camera.cy: 559.46813 12 | 13 | Camera.k1: 0.13335 14 | Camera.k2: -0.19215 15 | Camera.p1: 0.00017 16 | Camera.p2: -0.00712 17 | 18 | Camera.width: 1920 19 | Camera.height: 1080 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 25 | Camera.RGB: 0 26 | 27 | #-------------------------------------------------------------------------------------------- 28 | # ORB Parameters 29 | #-------------------------------------------------------------------------------------------- 30 | 31 | # ORB Extractor: Number of features per image 32 | ORBextractor.nFeatures: 1600 33 | 34 | # ORB Extractor: Scale factor between levels in the scale pyramid 35 | ORBextractor.scaleFactor: 1.2 36 | 37 | # ORB Extractor: Number of levels in the scale pyramid 38 | ORBextractor.nLevels: 8 39 | 40 | # ORB Extractor: Fast threshold 41 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 42 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 43 | # You can lower these values if your images have low contrast 44 | ORBextractor.iniThFAST: 20 45 | ORBextractor.minThFAST: 7 46 | 47 | #-------------------------------------------------------------------------------------------- 48 | # Viewer Parameters 49 | #-------------------------------------------------------------------------------------------- 50 | Viewer.KeyFrameSize: 0.05 51 | Viewer.KeyFrameLineWidth: 1 52 | Viewer.GraphLineWidth: 0.9 53 | Viewer.PointSize: 2 54 | Viewer.CameraSize: 0.08 55 | Viewer.CameraLineWidth: 3 56 | Viewer.ViewpointX: 0 57 | Viewer.ViewpointY: -0.7 58 | Viewer.ViewpointZ: -1.8 59 | Viewer.ViewpointF: 500 60 | 61 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/ptgrey/orbslam_outdoors_ptgrey.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 2406.442617 9 | Camera.fy: 2412.285633 10 | Camera.cx: 1202.854654 11 | Camera.cy: 1021.264401 12 | 13 | Camera.k1: -0.107492 14 | Camera.k2: 0.199451 15 | Camera.p1: -0.000891 16 | Camera.p2: 0.000976 17 | 18 | Camera.width: 2448 19 | Camera.height: 2048 20 | 21 | # Camera frames per second 22 | Camera.fps: 60.0 23 | 24 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 25 | Camera.RGB: 0 26 | 27 | #-------------------------------------------------------------------------------------------- 28 | # ORB Parameters 29 | #-------------------------------------------------------------------------------------------- 30 | 31 | # ORB Extractor: Number of features per image 32 | ORBextractor.nFeatures: 2000 33 | 34 | # ORB Extractor: Scale factor between levels in the scale pyramid 35 | ORBextractor.scaleFactor: 1.2 36 | 37 | # ORB Extractor: Number of levels in the scale pyramid 38 | ORBextractor.nLevels: 8 39 | 40 | # ORB Extractor: Fast threshold 41 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 42 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 43 | # You can lower these values if your images have low contrast 44 | ORBextractor.iniThFAST: 20 45 | ORBextractor.minThFAST: 7 46 | 47 | #-------------------------------------------------------------------------------------------- 48 | # Viewer Parameters 49 | #-------------------------------------------------------------------------------------------- 50 | Viewer.KeyFrameSize: 0.05 51 | Viewer.KeyFrameLineWidth: 1 52 | Viewer.GraphLineWidth: 0.9 53 | Viewer.PointSize: 2 54 | Viewer.CameraSize: 0.08 55 | Viewer.CameraLineWidth: 3 56 | Viewer.ViewpointX: 0 57 | Viewer.ViewpointY: -0.7 58 | Viewer.ViewpointZ: -1.8 59 | Viewer.ViewpointF: 500 60 | 61 | -------------------------------------------------------------------------------- /packages/flight/target_tracker_distributed_kf/launch/tracker.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /packages/flight/CMakeModules/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find Eigen3 3 | # 4 | # This sets the following variables: 5 | # EIGEN_FOUND - True if Eigen was found. 6 | # EIGEN_INCLUDE_DIRS - Directories containing the Eigen include files. 7 | # EIGEN_DEFINITIONS - Compiler flags for Eigen. 8 | # EIGEN_VERSION - Package version 9 | 10 | find_package(PkgConfig QUIET) 11 | pkg_check_modules(PC_EIGEN eigen3) 12 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 13 | 14 | if(CMAKE_SYSTEM_NAME STREQUAL Linux) 15 | set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} /usr /usr/local) 16 | endif(CMAKE_SYSTEM_NAME STREQUAL Linux) 17 | if(APPLE) 18 | list(APPEND CMAKE_INCLUDE_PATH /opt/local) 19 | set(CMAKE_FIND_FRAMEWORK NEVER) 20 | endif() 21 | 22 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 23 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" 24 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 25 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 26 | PATH_SUFFIXES eigen3 include/eigen3 include) 27 | 28 | if(EIGEN_INCLUDE_DIR) 29 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen_version_header) 30 | 31 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen_world_version_match "${_eigen_version_header}") 32 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 33 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen_major_version_match "${_eigen_version_header}") 34 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 35 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_version_match "${_eigen_version_header}") 36 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 37 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 38 | endif(EIGEN_INCLUDE_DIR) 39 | 40 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 41 | set(CMAKE_FIND_FRAMEWORK) 42 | 43 | include(FindPackageHandleStandardArgs) 44 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 45 | 46 | mark_as_advanced(EIGEN_INCLUDE_DIR) 47 | 48 | if(EIGEN_FOUND) 49 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS}, version: ${EIGEN_VERSION})") 50 | endif(EIGEN_FOUND) 51 | -------------------------------------------------------------------------------- /packages/replay/video_replay_only/Grab/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | videograb 4 | 0.0.0 5 | VideoGrab package 6 | 7 | 8 | 9 | 10 | raven 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | message_runtime 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | sensor_msgs 46 | roscpp 47 | std_msgs 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /packages/optional/basler_image_capture/Grab/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | videograb 4 | 0.0.0 5 | VideoGrab package 6 | 7 | 8 | 9 | 10 | raven 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | message_runtime 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | sensor_msgs 46 | roscpp 47 | std_msgs 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /packages/optional/ptgrey_image_capture/Grab/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | videograb 4 | 0.0.0 5 | VideoGrab package 6 | 7 | 8 | 9 | 10 | raven 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | message_runtime 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | sensor_msgs 46 | roscpp 47 | std_msgs 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /packages/simulation/fake_communication_failure/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fake_communication_failure 4 | 0.0.0 5 | The fake_communication_failure package 6 | 7 | 8 | 9 | 10 | EP 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rospy 44 | python-numpy 45 | 46 | rospy 47 | python-numpy 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/config/logitech_b500/orbslam_settings_logitech_b500.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 567.250 9 | Camera.fy: 568.046 10 | Camera.cx: 319.061 11 | Camera.cy: 211.211 12 | 13 | Camera.k1: -0.004553 14 | Camera.k2: -0.015001 15 | Camera.p1: 0.002478 16 | Camera.p2: 0.004511 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 1.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/include/model_distance_from_height/Projector.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 10.05.17. 3 | // 4 | 5 | #ifndef MODEL_DISTANCE_FROM_HEIGHT_PROJECTOR_H 6 | #define MODEL_DISTANCE_FROM_HEIGHT_PROJECTOR_H 7 | 8 | #undef DEBUG_PUBLISHERS 9 | 10 | #include "models/Model3D.h" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace model_distance_from_height { 23 | 24 | enum class Poses : int{ gpsoffset, robot, camera, optical }; 25 | 26 | class Projector { 27 | 28 | private: 29 | ros::NodeHandle nh_, pnh_{"~"}; 30 | ros::Subscriber detection_sub_, camera_info_sub_; 31 | ros::Publisher object_pose_pub_, camera_debug_pub_; 32 | 33 | image_geometry::PinholeCameraModel cameraModel_; 34 | std::unique_ptr> interface_; 35 | 36 | std::unique_ptr projectionModel_; 37 | 38 | std::vector> topics_; 39 | 40 | //Back-chain stuff 41 | ros::Subscriber tracker_sub_; 42 | ros::Publisher feedback_pub_; 43 | 44 | double head_uncertainty_scale_{1.0}; 45 | double feet_uncertainty_scale_{1.0}; 46 | 47 | //Dynamic reconfigure 48 | dynamic_reconfigure::Server dyn_rec_server_; 49 | 50 | #ifdef DEBUG_PUBLISHERS 51 | ros::Publisher debug_pub_; 52 | #endif 53 | 54 | public: 55 | Projector(); 56 | 57 | void detectionCallback3D(const neural_network_detector::NeuralNetworkDetectionArrayConstPtr &msg); 58 | void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg); 59 | void trackerCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg); 60 | 61 | void dynamicReconfigureCallback(ModelParametersConfig &config, uint32_t level); 62 | 63 | bool detectBackwardsTimeJump(); 64 | 65 | void interface_warning() const; 66 | }; 67 | 68 | } 69 | 70 | 71 | #endif //MODEL_DISTANCE_FROM_HEIGHT_PROJECTOR_H 72 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | random_moving_target 4 | 0.0.0 5 | The random_moving_target package 6 | 7 | 8 | 9 | 10 | aamir 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | roscpp 46 | rospy 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /packages/flight/pose_cov_ops_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pose_cov_ops_interface 4 | 0.0.0 5 | The pose_cov_ops_interface package 6 | 7 | 8 | 9 | 10 | Guilherme Lawless 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | message_filters 44 | pose_cov_ops 45 | roscpp 46 | message_filters 47 | pose_cov_ops 48 | roscpp 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/launch/one_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /packages/flight/camera_configs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | camera_configs 4 | 0.0.0 5 | The camera_configs package 6 | 7 | 8 | 9 | 10 | Guilherme Lawless 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | cv_camera 44 | camera_info_manager 45 | sensor_msgs 46 | cv_camera 47 | camera_info_manager 48 | sensor_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /packages/flight/tf_from_uav_pose/include/tf_from_uav_pose/tf_from_uav_pose.h: -------------------------------------------------------------------------------- 1 | // 2 | // tv_from_uav_poses_node.h 3 | // C++11 is required. Deal with it! 4 | // 5 | 6 | #ifndef TF_FROM_UAV_POSE_H 7 | #define TF_FROM_UAV_POSE_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | namespace tf_from_uav_pose { 22 | 23 | void uavCovariance_to_rosCovariance(const uav_msgs::uav_pose::ConstPtr &uav_msg, 24 | geometry_msgs::PoseWithCovariance &std_pose_cov); 25 | 26 | class tfFromUAVPose { 27 | 28 | private: 29 | ros::NodeHandle pnh_{"~"}; 30 | ros::NodeHandle nh_; 31 | 32 | ros::Subscriber poseSub_; 33 | ros::Subscriber rawPoseSub_; 34 | ros::Publisher stdPosePub_; 35 | ros::Publisher stdRawPosePub_; 36 | ros::Publisher throttledPosePub_; 37 | std::unique_ptr cameraPosePub_, camRGBPosePub_; 38 | std::unique_ptr tfBroadcaster_; 39 | std::unique_ptr statictfBroadcaster_; 40 | 41 | geometry_msgs::PoseWithCovarianceStamped stdPose_, stdRawPose_, camRobPose_, rgbCamPose_; 42 | geometry_msgs::PoseStamped throttledPose_; 43 | geometry_msgs::TransformStamped tfPose_; 44 | geometry_msgs::TransformStamped tfWorldENU_; 45 | geometry_msgs::TransformStamped tfWorldNWU_; 46 | geometry_msgs::TransformStamped tfCamRGB_; 47 | 48 | dynamic_reconfigure::Server dyn_rec_server_; 49 | 50 | ///@TODO remove this hacked offset, do it better 51 | std::vector offset_{0, 0, 0}; 52 | std::vector added_covariance_{0, 0, 0}; 53 | double throttleRate_{10.0}; 54 | 55 | bool dontPublishTFs_{false}; 56 | 57 | public: 58 | tfFromUAVPose(); 59 | 60 | void dynamicReconfigureCallback(ReconfigureParamsConfig &config, uint32_t level); 61 | 62 | void poseCallback(const uav_msgs::uav_pose::ConstPtr &msg); 63 | void rawPoseCallback(const uav_msgs::uav_pose::ConstPtr &msg); 64 | }; 65 | } 66 | 67 | #endif //TF_FROM_UAV_POSE_H 68 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/include/neural_network_detector/Overlay.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 27.04.17. 3 | // 4 | 5 | #ifndef NEURAL_NETWORK_DETECTOR_OVERLAY_H 6 | #define NEURAL_NETWORK_DETECTOR_OVERLAY_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace neural_network_detector { 25 | 26 | class Overlay { 27 | private: 28 | ros::NodeHandle pnh_{"~"}, nh_; 29 | message_filters::Subscriber img_sub_; 30 | message_filters::Subscriber feedback_sub_; 31 | message_filters::Cache feedback_cache_; 32 | message_filters::Subscriber detections_sub_; 33 | std::unique_ptr< message_filters::TimeSequencer > pimg_delayer_; 34 | std::unique_ptr< message_filters::TimeSynchronizer > pimg_detection_sync_; 35 | 36 | std::map stamp_detection_map_; 37 | 38 | image_transport::Publisher overlay_pub_; 39 | cv::Mat mat_img_; 40 | ros::Duration timeout_; 41 | 42 | std::unique_ptr< cv::VideoWriter > video_writer_{nullptr}; 43 | 44 | std::string video_filename_{""}; 45 | bool save_video_flag_{false}; 46 | double fps_{0}; 47 | 48 | void draw_detections(cv::Mat &, const NeuralNetworkDetectionArray *); 49 | void fade_out_unzoomed(cv::Mat &, const cv::Rect&); 50 | bool detectBackwardsTimeJump(); 51 | 52 | public: 53 | cv::Size2i desired_resolution; 54 | 55 | // Constructor 56 | Overlay(); 57 | 58 | void registerNewDetectionImagePair(const NeuralNetworkDetectionArrayConstPtr &, const sensor_msgs::ImageConstPtr &); 59 | void imgCallback(const sensor_msgs::ImageConstPtr &); 60 | 61 | void startSubscribers(); 62 | }; 63 | 64 | } 65 | #endif //NEURAL_NETWORK_DETECTOR_NNDETECTOR_H 66 | -------------------------------------------------------------------------------- /scrips/startup.sh: -------------------------------------------------------------------------------- 1 | #/bin/bash 2 | 3 | LOGDIR=/tmp/log 4 | FLIGHTNAME=$1 5 | MACHINE=$2 6 | CCOUNT=$3 7 | SDATE=$( date "+%Y-%m-%d_%T" ) 8 | HOSTNAME=$( hostname ) 9 | if [ -z $CCOUNT ]; then 10 | echo "usage $0 [flightname] [copter ID] [copter count]" 11 | exit 12 | fi 13 | 14 | export FLIGHTNAME="$FLIGHTNAME" 15 | export MACHINE=$MACHINE 16 | export CCOUNT=$CCOUNT 17 | export LOGDIR=$LOGDIR 18 | echo "flightname is $FLIGHTNAME" 19 | 20 | #create logdir 21 | mkdir -p $LOGDIR 22 | 23 | #delete symlink 24 | filename="${LOGDIR}/${FLIGHTNAME}_${SDATE}_${MACHINE}_${HOSTNAME}" 25 | mkdir $filename 26 | rm ${LOGDIR}/current 27 | ln -s $filename ${LOGDIR}/current 28 | 29 | # set working directory 30 | cd ${HOME}/src/catkin_ws 31 | 32 | # setup system write cache control 33 | #sudo /usr/local/sbin/speedup.sh 34 | 35 | # compile catkin modules 36 | source devel/setup.bash 37 | catkin_make 38 | 39 | echo "built latest source, starting roscore ..." 40 | 41 | # start a screen for command execution 42 | #roscore >/dev/null 2>&1 & 43 | screen -d -m -S ROSCORE bash -i -c ./roscore.sh 44 | sleep 10; 45 | echo "roscore started, starting LibrePilot Telemetry Link..." 46 | screen -d -m -S TELEMETRY bash -i -c ./telemetry.sh 47 | screen -d -m -S ROSLINK bash -i -c ./roslink.sh 48 | sleep 10; 49 | echo "link established, starting ROS logging.." 50 | screen -d -m -S ROSBAGSELF bash -i -c ./log.sh 51 | #screen -d -m -S ROSBAGALL bash -i -c ./logall.sh 52 | screen -d -m -S BANDWIDTHLOG bash -i -c ./bandwidth.sh 53 | 54 | echo "starting transformation publisher..." 55 | screen -d -m -S TRANSFORMATIONS bash -i -c ./transformations.sh 56 | echo "starting projection node..." 57 | screen -d -m -S PROJECTION bash -i -c ./projection.sh 58 | echo "starting camera calibration publisher..." 59 | screen -d -m -S CAMERACONFIG bash -i -c ./camera_config.sh 60 | echo "starting swarm sensor fusion..." 61 | screen -d -m -S SENSORFUSION bash -i -c ./sensorfusion.sh 62 | echo "starting visual detection..." 63 | screen -d -m -S DETECTION bash -i -c ./detection.sh 64 | 65 | echo "starting navigation..." 66 | screen -d -m -S NMPC_PLANNER bash -i -c ./navigation.sh 67 | 68 | echo "starting differential gps link..." 69 | screen -d -m -S DIFF_GPS bash -i -c ./diffgps.sh 70 | 71 | echo "starting video" 72 | screen -d -m -S VIDEO bash -i -c ./video.sh 73 | 74 | echo "everything is running. waiting for shutdown" 75 | touch ${LOGDIR}/current/started 76 | 77 | while [ ! -e ${LOGDIR}/current/abort ]; do 78 | sleep 1; 79 | done 80 | echo "shutdown initiated - starting cleanup, stuff should only fail now..." >>${LOGDIR}/current/faillog 81 | ./cleanup.sh 82 | 83 | -------------------------------------------------------------------------------- /packages/flight/tf_from_uav_pose/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tf_from_uav_pose 4 | 0.0.0 5 | The tf_from_uav_pose package 6 | 7 | 8 | 9 | 10 | Guilherme Lawless 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | tf 45 | uav_msgs 46 | geometry_msgs 47 | mrpt_bridge 48 | dynamic_reconfigure 49 | 50 | roscpp 51 | tf 52 | uav_msgs 53 | geometry_msgs 54 | mrpt_bridge 55 | dynamic_reconfigure 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /packages/flight/pose_cov_ops_interface/test/test_interface.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 11.05.17. 3 | // 4 | 5 | #include 6 | #include 7 | 8 | using namespace pose_cov_ops::interface; 9 | 10 | int main(int argc, char* argv[]){ 11 | ros::init(argc, argv, "pose_cov_ops_interface_test"); 12 | std::vector keys; 13 | std::vector> v; 14 | geometry_msgs::PoseWithCovariance p_out; 15 | 16 | // if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { 17 | // ros::console::notifyLoggerLevelsChanged(); 18 | // } 19 | 20 | // Get test topics from argv, roughly done 21 | if(argc < 4){ 22 | ROS_INFO("Usage: rosrun pose_cov_ops_interface pose_cov_ops_interface_test NAMESPACE TOPIC1 TOPIC2 ... "); 23 | return EXIT_FAILURE; 24 | } 25 | 26 | ros::NodeHandle n(argv[1]); 27 | 28 | for(int n_params=2; n_params < argc; ++n_params){ 29 | ROS_INFO_STREAM("Registering topic " << argv[n_params]); 30 | keys.emplace_back(argv[n_params]); 31 | v.emplace_back(topicSubInfo(keys.back(), keys.back(), 100, 10)); 32 | } 33 | 34 | // Register interface and print its contents 35 | Interface anInterface(v, n); 36 | ROS_INFO_STREAM(anInterface); 37 | 38 | // Wait for clock 39 | ros::Time::waitForValid(); 40 | 41 | ROS_INFO("Will repeatedly present the results of forward propagation for a vector [0 0 -1]"); 42 | geometry_msgs::Point point; 43 | point.z = -1.0; 44 | 45 | ros::Publisher resultPub = n.advertise("/machine_1/object_detections/pose_in_camera_frame", 5, false); 46 | geometry_msgs::PoseWithCovarianceStamped msg; 47 | msg.header.frame_id = "machine_1_camera_rgb_optical_link"; 48 | 49 | ros::Publisher resultWorldPub = n.advertise("/machine_1/object_detections/pose_in_world_frame", 5, false); 50 | geometry_msgs::PoseWithCovarianceStamped msgWorld; 51 | msgWorld.header.frame_id = "world"; 52 | msgWorld.pose.pose.orientation.w = 1.0; 53 | msgWorld.pose.pose.position = point; 54 | 55 | while(ros::ok()){ 56 | ros::spinOnce(); 57 | auto time_now = ros::Time::now(); 58 | if(anInterface.compose_down(point, keys, time_now, p_out)) 59 | ROS_INFO_STREAM(std::endl << p_out.pose.position); 60 | else{ 61 | ROS_INFO_STREAM("Not found at time " << time_now); 62 | } 63 | msg.header.stamp = time_now; 64 | msg.pose = p_out; 65 | resultPub.publish(msg); 66 | 67 | msgWorld.header.stamp = time_now; 68 | resultWorldPub.publish(msgWorld); 69 | 70 | ROS_DEBUG_STREAM(anInterface); 71 | ros::Rate(30).sleep(); 72 | } 73 | } -------------------------------------------------------------------------------- /packages/optional/gcs_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gcs_visualization 4 | 0.0.0 5 | The gcl_visualization package 6 | 7 | 8 | 9 | 10 | gsl 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rospy 44 | std_msgs 45 | geometry_msgs 46 | visualization_msgs 47 | tf 48 | python-numpy 49 | uav_msgs 50 | 51 | rospy 52 | std_msgs 53 | geometry_msgs 54 | visualization_msgs 55 | tf 56 | python-numpy 57 | uav_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /packages/flight/tf_from_uav_pose/launch/for_one_robot.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 | [0.18, 0.0, -0.07, 0.0, -0.38, 0.0, 0.924] 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /packages/simulation/librepilot_gazebo_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | librepilot_gazebo_bridge 4 | 0.0.0 5 | The librepilot_gazebo_bridge package 6 | 7 | 8 | 9 | 10 | EP 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | rospy 44 | std_msgs 45 | geometry_msgs 46 | visualization_msgs 47 | tf 48 | python-numpy 49 | uav_msgs 50 | 51 | rospy 52 | std_msgs 53 | geometry_msgs 54 | visualization_msgs 55 | tf 56 | python-numpy 57 | uav_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /packages/flight/target_tracker_distributed_kf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | target_tracker_distributed_kf 4 | 0.0.0 5 | The target_tracker_distributed_kf package 6 | 7 | 8 | 9 | 10 | glawless 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | geometry_msgs 44 | roscpp 45 | std_msgs 46 | eigen 47 | dynamic_reconfigure 48 | uav_msgs 49 | geometry_msgs 50 | roscpp 51 | std_msgs 52 | eigen 53 | dynamic_reconfigure 54 | uav_msgs 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/include/PotentialFunctionImpl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PotentialFunctionImpl.hpp 3 | * 4 | * Created on: Sep 8, 2012 5 | * Author: mriedel 6 | */ 7 | 8 | #ifndef POTENTIALFUNCTIONIMPL_HPP_ 9 | #define POTENTIALFUNCTIONIMPL_HPP_ 10 | 11 | #include 12 | #include 13 | 14 | namespace TELEKYB_NAMESPACE { 15 | 16 | namespace PotentialFunctionImpl { 17 | 18 | class CoTanRepulsiveGradient { 19 | private: 20 | PotentialFunctionOptions& options; 21 | public: 22 | CoTanRepulsiveGradient(PotentialFunctionOptions& options_) : options(options_) {} 23 | double operator()(double d, double threshold) const { 24 | double z = (M_PI/2.0) * 25 | ((d - options.tPotFuncInfD->getValue()) 26 | / (threshold - options.tPotFuncInfD->getValue())); 27 | double cot_z = 1.0/tan(z); 28 | double retValue = (M_PI/2.0) * (1.0 / (threshold - options.tPotFuncInfD->getValue())) * 29 | pow((cot_z + z - (M_PI/2.0)),options.tPotFuncGain->getValue()); 30 | return retValue; 31 | } 32 | PotentialFunctionType operator()() const { 33 | return PotentialFunctionType::Repulsive; 34 | } 35 | }; 36 | 37 | class CoTanAttractiveGradient { 38 | private: 39 | PotentialFunctionOptions& options; 40 | public: 41 | CoTanAttractiveGradient(PotentialFunctionOptions& options_) : options(options_) {} 42 | double operator()(double d, double threshold) const { 43 | double z = (M_PI/2.0) * 44 | ((threshold - d) 45 | / (threshold - options.tPotFuncZeroD->getValue())); 46 | double cot_z = 1.0/tan(z); 47 | double retValue = (M_PI/2.0) * (1.0 / (threshold - options.tPotFuncZeroD->getValue())) * 48 | pow((cot_z + z - (M_PI/2.0)),options.tPotFuncGain->getValue()); 49 | return retValue; 50 | } 51 | PotentialFunctionType operator()() const { 52 | return PotentialFunctionType::Attractive; 53 | } 54 | }; 55 | 56 | // Derivative of Gradient 57 | 58 | class CoTanRepulsiveHassian { 59 | private: 60 | PotentialFunctionOptions& options; 61 | public: 62 | CoTanRepulsiveHassian(PotentialFunctionOptions& options_) : options(options_) {} 63 | double operator()(double d, double threshold) const { 64 | double z = (M_PI/2.0) * 65 | ((d - options.tPotFuncInfD->getValue()) 66 | / (threshold - options.tPotFuncInfD->getValue())); 67 | double cot_z = 1.0/tan(z); 68 | double retValue = (M_PI/2.0) * (1.0 / (threshold - options.tPotFuncInfD->getValue())) * 69 | options.tPotFuncGain->getValue() * 70 | pow((cot_z + z - (M_PI/2.0)), options.tPotFuncGain->getValue() - 1.0) * 71 | pow(cot_z,2); 72 | return retValue; 73 | } 74 | PotentialFunctionType operator()() const { 75 | return PotentialFunctionType::Repulsive; 76 | } 77 | }; 78 | 79 | } 80 | 81 | } 82 | 83 | 84 | 85 | 86 | #endif /* POTENTIALFUNCTIONIMPL_HPP_ */ 87 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | neural_network_detector 4 | 0.0.0 5 | The neural_network_detector package 6 | 7 | 8 | 9 | 10 | Guilherme Lawless 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | cv_bridge 44 | image_transport 45 | roscpp 46 | sensor_msgs 47 | std_msgs 48 | message_filters 49 | message_generation 50 | cv_bridge 51 | image_transport 52 | roscpp 53 | sensor_msgs 54 | std_msgs 55 | message_filters 56 | message_runtime 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/src/PotentialFunction.cpp: -------------------------------------------------------------------------------- 1 | #include <../include/PotentialFunction.hpp> 2 | #include 3 | 4 | 5 | 6 | 7 | PotentialFunction::PotentialFunction(const std::string& potentialFunctionName_, PotentialFunctionType type_): type(type_) 8 | { 9 | 10 | updateBoundsValues(); 11 | 12 | } 13 | 14 | 15 | PotentialFunction::PotentialFunction(const std::string& potentialFunctionName_, PotentialFunctionType type_, double tPotFuncZeroD_, double tPotFuncInfD_, double tPotFuncSatValue_, double tPotFuncGain_): type(type_) 16 | { 17 | 18 | // set check automatic 19 | tPotFuncZeroD = tPotFuncZeroD_; 20 | tPotFuncInfD = tPotFuncInfD_; 21 | tPotFuncSatValue = tPotFuncSatValue_; 22 | tPotFuncGain = tPotFuncGain_; 23 | 24 | // no callbacks implemented yet 25 | updateBoundsValues(); 26 | 27 | 28 | } 29 | 30 | 31 | void PotentialFunction::updateBoundsValues() 32 | { 33 | // ROS_INFO("Called updateBoundsValues()"); 34 | //PotentialFunctionType type = getType(); 35 | 36 | double zeroDistance = tPotFuncZeroD; 37 | double infDistance = tPotFuncInfD; 38 | 39 | // ROS_INFO("Values: tPotFuncZeroD: %f, tPotFuncInfD: %f", zeroDistance, infDistance); 40 | 41 | // Beware: Do not check for equality here. Otherwise: Infinite loop. 42 | if (zeroDistance < infDistance && type == PotentialFunctionType::Repulsive) 43 | { 44 | ROS_ERROR("Potential Function Type %d, with d_0(%f) < d_inf(%f). Inverting...", type, zeroDistance, infDistance); 45 | tPotFuncZeroD = infDistance; 46 | tPotFuncInfD = zeroDistance; 47 | } 48 | else 49 | if (infDistance < zeroDistance && type == PotentialFunctionType::Attractive) 50 | { 51 | ROS_ERROR("Potential Function Type %d, with d_0(%f) > d_inf(%f). Inverting...", type, zeroDistance, infDistance); 52 | tPotFuncZeroD = infDistance; 53 | tPotFuncInfD = zeroDistance; 54 | } 55 | else 56 | { 57 | // everything ok; 58 | } 59 | } 60 | 61 | PotentialFunctionType PotentialFunction::getType() const { 62 | return type; 63 | } 64 | 65 | 66 | 67 | double PotentialFunction::getPotential(double d, double threshold) const 68 | { 69 | 70 | //ROS_INFO("........Value: %f, sat value: %f",d, tPotFuncSatValue); 71 | 72 | if (type == PotentialFunctionType::Repulsive) { 73 | // repulsive 74 | if (d < tPotFuncInfD) { 75 | return tPotFuncSatValue; 76 | } else if (d >= threshold/*tPotFuncZeroD*/) { 77 | return 0.0; 78 | } else { 79 | return std::min(getPotentialImpl(d,threshold), tPotFuncSatValue); 80 | } 81 | 82 | } else { 83 | // attractive 84 | if (d <= threshold/*tPotFuncZeroD*/) { 85 | return 0.0; 86 | } else if (d > tPotFuncInfD) { 87 | return tPotFuncSatValue; 88 | } else { 89 | return std::min(getPotentialImpl(d,threshold), tPotFuncSatValue); 90 | } 91 | } 92 | } 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /packages/simulation/random_moving_target/src/virtual_target_square_motion.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include "geometry_msgs/Twist.h" 4 | #include 5 | #include "std_msgs/String.h" 6 | 7 | 8 | /** 9 | * This tutorial demonstrates simple sending of velocity commands to the IRobot Create in Gazebo. 10 | */ 11 | int main(int argc, char **argv) 12 | { 13 | 14 | ros::init(argc, argv, "CreateController"); 15 | 16 | ros::NodeHandle n; 17 | 18 | ros::Publisher virtual_point_publisher = n.advertise("/virtual_target_square_motion", 10); 19 | 20 | int loop_rate_value = 30; 21 | ros::Rate loop_rate(loop_rate_value); 22 | 23 | geometry_msgs::PoseWithCovarianceStamped msg; 24 | 25 | int count = 0; 26 | double squareSide = 4.0; // in m 27 | double speed = 0.3; //in m/s 28 | 29 | double timeTakenPerSide = squareSide/speed; 30 | 31 | int iterationsPerSide = timeTakenPerSide*loop_rate_value; 32 | 33 | int squareSideNUmber = 1; 34 | int counter = 0; 35 | 36 | while (ros::ok()) 37 | { 38 | msg.header.stamp = ros::Time::now(); 39 | msg.header.frame_id = "world_link"; 40 | 41 | if(squareSideNUmber == 1) 42 | { 43 | msg.pose.pose.position.x = counter*(squareSide/iterationsPerSide); 44 | msg.pose.pose.position.y = 0; 45 | msg.pose.pose.position.z = 0; 46 | counter++; 47 | 48 | virtual_point_publisher.publish(msg); 49 | if(counter>iterationsPerSide) 50 | { 51 | counter = 0; 52 | squareSideNUmber = 2; 53 | } 54 | } 55 | 56 | if(squareSideNUmber == 2) 57 | { 58 | msg.pose.pose.position.x = squareSide; 59 | msg.pose.pose.position.y = counter*(squareSide/iterationsPerSide); 60 | msg.pose.pose.position.z = 0; 61 | counter++; 62 | 63 | virtual_point_publisher.publish(msg); 64 | if(counter>iterationsPerSide) 65 | { 66 | counter = iterationsPerSide; 67 | squareSideNUmber = 3; 68 | } 69 | } 70 | 71 | if(squareSideNUmber == 3) 72 | { 73 | msg.pose.pose.position.x = counter*(squareSide/iterationsPerSide); 74 | msg.pose.pose.position.y = squareSide; 75 | msg.pose.pose.position.z = 0; 76 | counter--; 77 | 78 | virtual_point_publisher.publish(msg); 79 | if(counter<0) 80 | { 81 | counter = iterationsPerSide; 82 | squareSideNUmber = 4; 83 | } 84 | } 85 | 86 | if(squareSideNUmber == 4) 87 | { 88 | msg.pose.pose.position.x = 0; 89 | msg.pose.pose.position.y = counter*(squareSide/iterationsPerSide); 90 | msg.pose.pose.position.z = 0; 91 | counter--; 92 | 93 | virtual_point_publisher.publish(msg); 94 | if(counter<0) 95 | { 96 | counter = 0; 97 | squareSideNUmber = 1; 98 | } 99 | } 100 | 101 | ros::spinOnce(); 102 | 103 | loop_rate.sleep(); 104 | 105 | ++count; 106 | } 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/launch/planner_activeTracking_square.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /packages/flight/generic_potential_field/src/CoTanPotentialFunctions.cpp: -------------------------------------------------------------------------------- 1 | #include <../include/CoTanPotentialFunctions.hpp> 2 | 3 | CoTanRepulsiveGradient::CoTanRepulsiveGradient(const std::string& potentialFunctionName_) 4 | : PotentialFunction(potentialFunctionName_, PotentialFunctionType::Repulsive) 5 | { 6 | 7 | } 8 | 9 | CoTanRepulsiveGradient::CoTanRepulsiveGradient(const std::string& potentialFunctionName_, 10 | double tPotFuncZeroD_, double tPotFuncInfD_, double tPotFuncSatValue_, double tPotFuncGain_) 11 | : PotentialFunction(potentialFunctionName_, PotentialFunctionType::Repulsive, 12 | tPotFuncZeroD_, tPotFuncInfD_, tPotFuncSatValue_, tPotFuncGain_) 13 | { 14 | 15 | } 16 | 17 | double CoTanRepulsiveGradient::getPotentialImpl(double d, double threshold) const { 18 | 19 | //tPotFuncZeroD = threshold; 20 | double z = (M_PI/2.0) * 21 | ((d - tPotFuncInfD) 22 | / (threshold - tPotFuncInfD)); 23 | double cot_z = cos(z)/sin(z); 24 | double retValue = (M_PI/2.0) * (1.0 / (threshold - tPotFuncInfD)) * 25 | (cot_z + z - (M_PI/2.0)); 26 | // pow((cot_z + z - (M_PI/2.0)),tPotFuncGain); 27 | return tPotFuncGain * retValue; 28 | } 29 | 30 | 31 | 32 | 33 | CoTanAttractiveGradient::CoTanAttractiveGradient(const std::string& potentialFunctionName_) 34 | : PotentialFunction(potentialFunctionName_, PotentialFunctionType::Attractive) 35 | { 36 | 37 | /* dynamic_reconfigure::Server::CallbackType callback; 38 | callback = boost::bind(&CoTanAttractiveGradient::reconf_callback, this, _1); 39 | 40 | dynamicServer_.setCallback(callback); */ 41 | 42 | } 43 | 44 | CoTanAttractiveGradient::CoTanAttractiveGradient(const std::string& potentialFunctionName_, 45 | double tPotFuncZeroD_, double tPotFuncInfD_, double tPotFuncSatValue_, double tPotFuncGain_) 46 | : PotentialFunction(potentialFunctionName_, PotentialFunctionType::Attractive, 47 | tPotFuncZeroD_, tPotFuncInfD_, tPotFuncSatValue_, tPotFuncGain_) 48 | { 49 | 50 | /* dynamic_reconfigure::Server::CallbackType callback; 51 | callback = boost::bind(&CoTanAttractiveGradient::reconf_callback, this, _1); 52 | 53 | dynamicServer_.setCallback(callback); */ 54 | 55 | } 56 | 57 | double CoTanAttractiveGradient::getPotentialImpl(double d, double threshold) const { 58 | 59 | //tPotFuncInfD = threshold; 60 | double z = (M_PI/2.0) * 61 | ((threshold - d) 62 | / (threshold - tPotFuncZeroD)); 63 | double cot_z = cos(z)/sin(z); 64 | double retValue = (M_PI/2.0) * (1.0 / (threshold - tPotFuncZeroD)) * 65 | (cot_z + z - (M_PI/2.0)); 66 | // pow((cot_z + z - (M_PI/2.0)),tPotFuncGain); 67 | return tPotFuncGain * retValue; 68 | } 69 | 70 | 71 | // void CoTanAttractiveGradient::reconf_callback(generic_potential_field::potentialParamsConfig &config) 72 | // { 73 | // tPotFuncZeroD=config.tPotFuncZeroD; 74 | // tPotFuncInfD=config.tPotFuncInfD; 75 | // tPotFuncSatValue=config.tPotFuncSatValue; 76 | // tPotFuncGain=config.tPotFuncGain; 77 | // 78 | // ROS_INFO("...WHILE CoTanAttractiveGradient CHANGING.....Value:, sat value: %f", tPotFuncSatValue); 79 | // 80 | // } -------------------------------------------------------------------------------- /packages/flight/projection_models/model_distance_from_height/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | model_distance_from_height 4 | 0.0.0 5 | The model_distance_from_height package 6 | 7 | 8 | 9 | 10 | Guilherme Lawless 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | geometry_msgs 43 | image_geometry 44 | neural_network_detector 45 | roscpp 46 | tf2_ros 47 | sensor_msgs 48 | pose_cov_ops 49 | pose_cov_ops_interface 50 | dynamic_reconfigure 51 | geometry_msgs 52 | image_geometry 53 | neural_network_detector 54 | roscpp 55 | tf2_ros 56 | sensor_msgs 57 | pose_cov_ops 58 | pose_cov_ops_interface 59 | dynamic_reconfigure 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /packages/flight/neural_network_detector/include/neural_network_detector/NNDetector.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by glawless on 27.04.17. 3 | // 4 | 5 | #ifndef NEURAL_NETWORK_DETECTOR_NNDETECTOR_H 6 | #define NEURAL_NETWORK_DETECTOR_NNDETECTOR_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace neural_network_detector { 21 | 22 | typedef struct __attribute__ ((__packed__)) { 23 | uint8_t label; 24 | float score; 25 | int16_t xmin; 26 | int16_t xmax; 27 | int16_t ymin; 28 | int16_t ymax; 29 | } detection_info; 30 | 31 | 32 | typedef struct __attribute__ ((__packed__)) { 33 | uint16_t count; 34 | detection_info detection[INT_MAX]; // over_allocated since variable length arrays are not allowed in C++ 35 | } detection_results; 36 | 37 | cv::Rect get_crop_area(const NeuralNetworkFeedback &latest_feedback, const cv::Size2i &original_resolution, const cv::Size2i &desired_resolution, cv::projection2i& proj_crop, bool timed_out); 38 | 39 | class NNDetector { 40 | private: 41 | std::unique_ptr c_{ 42 | new BoostTCPClient}; // will call destructor when out of scope, closing connection smoothly 43 | ros::NodeHandle pnh_{"~"}, nh_; 44 | image_transport::Subscriber img_sub_; 45 | cv::Mat mat_img_; 46 | std::string host_, port_; 47 | size_t length_final_img_; 48 | std::unique_ptr buffer_final_img_, buffer_results_; // will automatically delete when out of scope 49 | ros::Publisher detection_pub_; 50 | image_transport::Publisher debug_result_pub_; 51 | ros::Subscriber feedback_sub_; 52 | neural_network_detector::NeuralNetworkFeedback latest_feedback_; 53 | ros::Duration timeout_; 54 | ros::Publisher detection_amount_pub_; 55 | double border_dropoff_{.05}; 56 | 57 | // Connect to TCP server (NN) 58 | bool connect(); 59 | 60 | // Try multiple times to connect 61 | bool connectMultiple(ros::Rate sleeper, int tries); 62 | 63 | public: 64 | cv::Size2i desired_resolution; 65 | float score_threshold{0.5}; 66 | int desired_class{15}; 67 | float var_const_x_min{0}, var_const_x_max{0}, var_const_y_min{0}, var_const_y_max{0}; 68 | 69 | bool max_update_force{false}; 70 | std::unique_ptr max_update_rate; 71 | 72 | // Constructor 73 | NNDetector(char *host, char *port); 74 | 75 | // Image callback in which communication is handled as well 76 | void imgCallback(const sensor_msgs::ImageConstPtr &); 77 | 78 | // Disconnect callback to print disconnect message 79 | void connectCallback(); 80 | 81 | // Feedback callback for updating latest feedback info 82 | void feedbackCallback(const neural_network_detector::NeuralNetworkFeedbackConstPtr& msg); 83 | }; 84 | 85 | } 86 | #endif //NEURAL_NETWORK_DETECTOR_NNDETECTOR_H 87 | -------------------------------------------------------------------------------- /packages/flight/diff_gps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | diff_gps 4 | 0.0.0 5 | Package for reading EMLID serial gps data 6 | 7 | 8 | 9 | 10 | igor 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | message_generation 56 | geometry_msgs 57 | roscpp 58 | rospy 59 | std_msgs 60 | geometry_msgs 61 | roscpp 62 | rospy 63 | std_msgs 64 | geometry_msgs 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /packages/flight/nmpc_planner/include/solver/util.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-12-22 06:28:40 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: util.c. */ 8 | /* Description: Common utility file for all cvxgen code. */ 9 | #include "solver.h" 10 | #include 11 | #include 12 | #include 13 | long global_seed = 1; 14 | static clock_t tic_timestart; 15 | void tic(void) { 16 | tic_timestart = clock(); 17 | } 18 | float toc(void) { 19 | clock_t tic_timestop; 20 | tic_timestop = clock(); 21 | printf("time: %8.2f.\n", (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC); 22 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 23 | } 24 | float tocq(void) { 25 | clock_t tic_timestop; 26 | tic_timestop = clock(); 27 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 28 | } 29 | void printmatrix(char *name, double *A, int m, int n, int sparse) { 30 | int i, j; 31 | printf("%s = [...\n", name); 32 | for (i = 0; i < m; i++) { 33 | for (j = 0; j < n; j++) 34 | if ((sparse == 1) && (A[i+j*m] == 0)) 35 | printf(" 0"); 36 | else 37 | printf(" % 9.4f", A[i+j*m]); 38 | printf(",\n"); 39 | } 40 | printf("];\n"); 41 | } 42 | double unif(double lower, double upper) { 43 | return lower + ((upper - lower)*rand())/RAND_MAX; 44 | } 45 | /* Next function is from numerical recipes in C. */ 46 | #define IA 16807 47 | #define IM 2147483647 48 | #define AM (1.0/IM) 49 | #define IQ 127773 50 | #define IR 2836 51 | #define NTAB 32 52 | #define NDIV (1+(IM-1)/NTAB) 53 | #define EPS 1.2e-7 54 | #define RNMX (1.0-EPS) 55 | float ran1(long*idum, int reset) { 56 | int j; 57 | long k; 58 | static long iy=0; 59 | static long iv[NTAB]; 60 | float temp; 61 | if (reset) { 62 | iy = 0; 63 | } 64 | if (*idum<=0||!iy) { 65 | if (-(*idum)<1)*idum=1; 66 | else *idum=-(*idum); 67 | for (j=NTAB+7; j>=0; j--) { 68 | k = (*idum)/IQ; 69 | *idum=IA*(*idum-k*IQ)-IR*k; 70 | if (*idum<0)*idum+=IM; 71 | if (j RNMX) return RNMX; 82 | else return temp; 83 | } 84 | /* Next function is from numerical recipes in C. */ 85 | float randn_internal(long *idum, int reset) { 86 | static int iset=0; 87 | static float gset; 88 | float fac, rsq, v1, v2; 89 | if (reset) { 90 | iset = 0; 91 | } 92 | if (iset==0) { 93 | do { 94 | v1 = 2.0*ran1(idum, reset)-1.0; 95 | v2 = 2.0*ran1(idum, reset)-1.0; 96 | rsq = v1*v1+v2*v2; 97 | } while(rsq >= 1.0 || rsq == 0.0); 98 | fac = sqrt(-2.0*log(rsq)/rsq); 99 | gset = v1*fac; 100 | iset = 1; 101 | return v2*fac; 102 | } else { 103 | iset = 0; 104 | return gset; 105 | } 106 | } 107 | double randn(void) { 108 | return randn_internal(&global_seed, 0); 109 | } 110 | void reset_rand(void) { 111 | srand(15); 112 | global_seed = 1; 113 | randn_internal(&global_seed, 1); 114 | } 115 | --------------------------------------------------------------------------------