├── autonomy_sim ├── unity_sim │ ├── CATKIN_IGNORE │ ├── fake_lidar │ │ ├── launch │ │ │ ├── debug.conf │ │ │ ├── os1-128.yaml │ │ │ ├── os1-64.yaml │ │ │ ├── vlp16.yaml │ │ │ ├── depth2cloud.launch │ │ │ ├── fake_lidar_cloud.launch │ │ │ └── fake_lidar_semantic_cloud.launch │ │ ├── package.xml │ │ ├── src │ │ │ └── utils.h │ │ └── CMakeLists.txt │ ├── docker │ │ ├── build.sh │ │ ├── run.sh │ │ └── entrypoint.sh │ ├── fake_sloam │ │ ├── CMakeLists.txt │ │ └── package.xml │ ├── dcist_utils │ │ ├── config │ │ │ ├── forest.yaml │ │ │ ├── overpasscity_demo.yaml │ │ │ └── emu_stereo_lidar_old_format.yaml │ │ ├── launch │ │ │ ├── README_MSCKF_CALIB_GEN.txt │ │ │ ├── sim │ │ │ │ ├── vel_control_for_gain_tuning.launch │ │ │ │ ├── full_sim_jetson_nuc.launch │ │ │ │ ├── sim_quad_polypixel.launch │ │ │ │ ├── sim_quad_overpass_customized.launch │ │ │ │ └── sim_quad.launch │ │ │ ├── rosflight_interface.launch │ │ │ ├── msckf_calib_gen.launch │ │ │ ├── odom2tf.launch │ │ │ └── rosflight.launch │ │ ├── src │ │ │ └── playground.cpp │ │ ├── package.xml │ │ ├── script │ │ │ └── takeoff.bash │ │ └── CMakeLists.txt │ └── Dockerfile ├── docker │ ├── build.sh │ ├── run.sh │ └── entrypoint.sh ├── gazebo_sim │ └── gazebo_utils │ │ ├── CMakeLists.txt │ │ ├── config │ │ └── falcon4_os1_so3_gains.yaml │ │ ├── package.xml │ │ └── launch │ │ └── simulation.launch ├── external.yaml └── Dockerfile ├── autonomy_core ├── client │ ├── rqt_quadrotor_safety │ │ ├── src │ │ │ └── rqt_quadrotor_safety │ │ │ │ └── __init__.py │ │ ├── scripts │ │ │ └── rqt_quadrotor_safety │ │ ├── setup.py │ │ ├── CMakeLists.txt │ │ ├── plugin.xml │ │ └── package.xml │ ├── docker │ │ ├── build.sh │ │ ├── run.sh │ │ ├── entrypoint.sh │ │ ├── deploy2.sh │ │ └── deploy.sh │ ├── client_launch │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── launch │ │ │ └── client.launch │ └── Dockerfile ├── map_plan │ ├── mapper │ │ ├── launch │ │ │ ├── debug.conf │ │ │ └── rviz.launch │ │ ├── include │ │ │ └── mapper │ │ │ │ └── tf_listener.h │ │ ├── package.xml │ │ └── src │ │ │ └── tf_listener.cpp │ ├── action_planner │ │ ├── launch │ │ │ ├── debug.conf │ │ │ ├── test.launch │ │ │ └── rviz.launch │ │ ├── src │ │ │ ├── data_conversions.h │ │ │ └── primitive_ros_utils.h │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ └── cfg │ │ │ └── ActionPlanner.cfg │ ├── coverage_utils │ │ ├── config │ │ │ └── input.txt │ │ ├── package.xml │ │ ├── include │ │ │ ├── coverage_utils.h │ │ │ └── utils.h │ │ ├── src │ │ │ ├── coverage_utils_node.cpp │ │ │ └── coverage_utils.cpp │ │ ├── launch │ │ │ └── coverage_utils.launch │ │ └── CMakeLists.txt │ ├── docker │ │ ├── build.sh │ │ ├── run.sh │ │ └── entrypoint.sh │ ├── jps3d │ │ ├── data │ │ │ ├── simple.png │ │ │ ├── corridor.png │ │ │ ├── example_dmp.png │ │ │ └── simple3d.yaml │ │ ├── package.xml │ │ ├── wercker.yml │ │ ├── CMakeLists.txt │ │ └── include │ │ │ └── jps │ │ │ └── data_utils.h │ ├── mpl │ │ ├── data │ │ │ ├── test_planner_2d.jpg │ │ │ ├── test_traj_solver.jpg │ │ │ ├── test_planner_2d_with_yaw.jpg │ │ │ ├── test_distance_map_planner_2d.jpg │ │ │ ├── test_planner_2d_with_prior_traj.jpg │ │ │ ├── test_distance_map_planner_2d_iterative.jpg │ │ │ └── test_distance_map_planner_2d_with_yaw.jpg │ │ ├── src │ │ │ ├── map_planner.cpp │ │ │ └── waypoint.cpp │ │ ├── script │ │ │ ├── optimal_acc_primitive.py │ │ │ ├── optimal_jerk_primitive.py │ │ │ └── test_hash.cpp │ │ ├── package.xml │ │ ├── wercker.yml │ │ └── include │ │ │ ├── mpl_planner │ │ │ └── map_planner.h │ │ │ └── mpl_basis │ │ │ ├── control.h │ │ │ ├── lambda.h │ │ │ └── math.h │ ├── map_plan_launch │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── planner.launch │ │ │ ├── depth_to_cloud.launch │ │ │ ├── mapper.launch │ │ │ ├── mp_planner.launch │ │ │ └── map_plan.launch │ │ └── package.xml │ ├── external-repos.yaml │ ├── traj_opt_ros │ │ ├── plugin_description.xml │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── traj_opt_ros │ │ │ │ ├── msg_traj.h │ │ │ │ ├── types.h │ │ │ │ ├── trajectory.h │ │ │ │ ├── ros_bridge.h │ │ │ │ └── traj_data.h │ │ └── src │ │ │ └── trajectory.cpp │ └── Dockerfile ├── base │ ├── docker │ │ ├── build.sh │ │ ├── run.sh │ │ └── entrypoint.sh │ └── Dockerfile ├── control │ ├── docker │ │ ├── build.sh │ │ ├── run.sh │ │ └── entrypoint.sh │ ├── control_launch │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── mav_manager_params.yaml │ │ │ ├── so3_control_gains.yaml │ │ │ └── trackers_mp.yaml │ │ └── package.xml │ ├── external-repos.yaml │ ├── README.txt │ └── Dockerfile ├── estimation │ ├── docker │ │ ├── build.sh │ │ ├── run.sh │ │ └── entrypoint.sh │ ├── external-repos.yaml │ ├── estimation_launch │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── ukf_params.yaml │ │ │ ├── msckf_calib_template.yaml │ │ │ └── msckf_calib_simulation.yaml │ │ ├── package.xml │ │ └── launch │ │ │ └── msckf_imgproc.launch │ ├── fla_ukf │ │ ├── nodelet_plugins.xml │ │ ├── config │ │ │ └── params.yaml │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ └── launch │ │ │ └── ukf.launch │ └── Dockerfile ├── state_machine │ ├── action_trackers │ │ ├── action │ │ │ ├── Land.action │ │ │ ├── TakeOff.action │ │ │ ├── GoalDistance.action │ │ │ ├── ShortRange.action │ │ │ ├── RunPath.action │ │ │ └── RunTrajectory.action │ │ ├── include │ │ │ └── action_trackers │ │ │ │ └── traj_to_quad_cmd.h │ │ └── package.xml │ ├── docker │ │ ├── build.sh │ │ ├── run.sh │ │ └── entrypoint.sh │ ├── state_machine_launch │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── launch │ │ │ ├── replanner.launch │ │ │ ├── state_machine.launch │ │ │ ├── smach.launch │ │ │ ├── state_machine_mp.launch │ │ │ └── system.launch │ ├── Dockerfile │ └── state_machine_core │ │ ├── action │ │ └── Replan.action │ │ ├── include │ │ └── state_machine │ │ │ └── traj_opt_utils.hpp │ │ ├── package.xml │ │ ├── src │ │ └── traj_opt_utils.cpp │ │ ├── CMakeLists.txt │ │ └── scripts │ │ └── Utils.py └── interface │ ├── docker │ ├── build.sh │ ├── run.sh │ └── entrypoint.sh │ ├── mavros_interface │ ├── README.md │ ├── nodelet_plugins.xml │ ├── params │ │ └── extras.txt │ ├── package.xml │ ├── CMakeLists.txt │ └── launch │ │ └── fla250.launch │ ├── px4_interface_launch │ ├── CMakeLists.txt │ ├── README.txt │ └── package.xml │ ├── Dockerfile │ └── scripts │ └── install_geographiclib_datasets.sh ├── docs ├── falcon4-compressed.jpg ├── autonomy_stack_pipeline.png └── ouster_metadata │ └── parse_ouster_metadata_from_rosbag.py ├── .clang-format ├── autonomy_real └── real_experiment_launch │ ├── CMakeLists.txt │ ├── launch │ ├── old-launch │ │ ├── static_tf_vision_world.launch │ │ ├── test_mapper.launch │ │ ├── record_bag_for_GUI.launch │ │ ├── throttle_odom_img.launch │ │ ├── sensing_only-old.launch │ │ ├── msckf_only.launch │ │ └── estimation_only.launch │ ├── relay_ovc.launch │ ├── throttle_map_odom.launch │ ├── record_bag.launch │ ├── publish_tf.launch │ ├── px4.launch │ └── ouster_decoder.launch │ ├── config │ ├── px4_pluginlists.yaml │ ├── waypoints │ │ ├── old-waypoints-icra-22-demo.yaml │ │ └── waypoints-new-icra-22-demo.yaml │ ├── calib_july_aug_2021 │ │ ├── apriltag.yaml │ │ ├── imu_for_kalibr.yaml │ │ ├── kalibr-8-30-2021 │ │ │ ├── result-1st-success-refined-reproj-error-0.5 │ │ │ │ └── imu-.calibration-8-30-2021cam-imu.yaml │ │ │ └── result-1st-success │ │ │ │ ├── initial-camera.yaml │ │ │ │ └── camchain-imucam-.calibration-8-30-2021cam-imu.yaml │ │ ├── msckf_calib-8-11.yaml │ │ ├── msckf_calib-8-23-final.yaml │ │ ├── msckf_calib_forest3.yaml │ │ ├── msckf_calib_forest2.yaml │ │ ├── msckf_calib_forest_1.yaml │ │ └── falcon4-multicam-output-2021-07-25.yaml │ ├── so3_control_gains.yaml │ ├── so3_gains_backup │ │ ├── so3_control_gains_nov_4_before_tuning.yaml │ │ └── so3_control_gains_oct_24_before_tuning.yaml │ ├── trackers.yaml │ ├── so3_params_explained.txt │ ├── tracker_params.yaml │ └── msckf_calib.yaml │ ├── package.xml │ └── util │ └── convert_multicam_calib_to_msckf.py ├── external_real_robot.yaml ├── external_coverage_planner.yaml ├── .github └── workflows │ ├── pylint-reviewdog.yaml │ ├── shellcheck-reviewdog.yaml │ ├── cpplint-reviewdog.yaml │ ├── docker-build-base.yaml │ ├── docker-build-estimation.yaml │ ├── docker-build-sim.yaml │ ├── docker-build-client.yaml │ ├── docker-build-control.yaml │ ├── docker-build-map-plan.yaml │ └── docker-build-state-machine.yaml ├── external_all.yaml └── .cmake-format.yaml /autonomy_sim/unity_sim/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /autonomy_core/client/rqt_quadrotor_safety/src/rqt_quadrotor_safety/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mapper/launch/debug.conf: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.mapper=INFO 2 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/launch/debug.conf: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.fake_lidar=DEBUG -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/launch/debug.conf: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.action_planner=DEBUG -------------------------------------------------------------------------------- /autonomy_core/base/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:base .. -------------------------------------------------------------------------------- /autonomy_sim/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:sim .. 4 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/config/input.txt: -------------------------------------------------------------------------------- 1 | 12k_giga_trees_nn_radius_2_shift_start_to_origin.txt -------------------------------------------------------------------------------- /autonomy_core/client/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:client .. 4 | -------------------------------------------------------------------------------- /autonomy_core/control/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:control .. 4 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:sim .. 4 | -------------------------------------------------------------------------------- /autonomy_core/estimation/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:estimation .. 4 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:map_plan .. 4 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/action/Land.action: -------------------------------------------------------------------------------- 1 | # goal 2 | --- 3 | # result 4 | bool success 5 | --- -------------------------------------------------------------------------------- /autonomy_core/interface/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:px4_interface .. 4 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker build -t kumarrobotics/autonomy:state_machine .. 4 | -------------------------------------------------------------------------------- /docs/falcon4-compressed.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/docs/falcon4-compressed.jpg -------------------------------------------------------------------------------- /docs/autonomy_stack_pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/docs/autonomy_stack_pipeline.png -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_sloam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(fake_sloam) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/data/simple.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/jps3d/data/simple.png -------------------------------------------------------------------------------- /autonomy_core/interface/mavros_interface/README.md: -------------------------------------------------------------------------------- 1 | # mavros_interface 2 | 3 | This node translates `kr_mav_msgs/SO3Command` to `mavros_msgs/AttitudeTarget`. 4 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/data/corridor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/jps3d/data/corridor.png -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/config/forest.yaml: -------------------------------------------------------------------------------- 1 | unity_command: 2 | unity_setup: 3 | Spawner: 4 | scene: "Assets/FloodedGrounds.unity" 5 | 6 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/data/example_dmp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/jps3d/data/example_dmp.png -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/data/test_planner_2d.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/mpl/data/test_planner_2d.jpg -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | BinPackArguments: false 4 | BinPackParameters: false 5 | PointerAlignment: Left 6 | DerivePointerAlignment: false 7 | -------------------------------------------------------------------------------- /autonomy_core/client/client_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(client_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | -------------------------------------------------------------------------------- /autonomy_core/estimation/external-repos.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | msckf_vio: 3 | type: git 4 | url: https://github.com/KumarRobotics/msckf_vio 5 | version: master 6 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/data/test_traj_solver.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/mpl/data/test_traj_solver.jpg -------------------------------------------------------------------------------- /autonomy_core/control/control_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(control_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker run -it --rm --network=host --name autonomy_map_plan_it \ 4 | kumarrobotics/autonomy:map_plan \ 5 | bash 6 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/map_plan_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(map_plan_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | -------------------------------------------------------------------------------- /autonomy_core/estimation/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker run -it --rm --network=host --name autonomy_estimation_it \ 4 | kumarrobotics/autonomy:estimation \ 5 | bash 6 | -------------------------------------------------------------------------------- /autonomy_core/estimation/estimation_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(estimation_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/data/test_planner_2d_with_yaw.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/mpl/data/test_planner_2d_with_yaw.jpg -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(real_experiment_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/external-repos.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | kr_planning_msgs: 3 | type: git 4 | url: https://github.com/KumarRobotics/kr_planning_msgs.git 5 | version: main 6 | 7 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/data/test_distance_map_planner_2d.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/mpl/data/test_distance_map_planner_2d.jpg -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/action/TakeOff.action: -------------------------------------------------------------------------------- 1 | # Action goal 2 | float32 height 3 | --- 4 | # Action result 5 | bool at_goal 6 | --- 7 | # Action status 8 | float32 time_remaining -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(state_machine_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/data/test_planner_2d_with_prior_traj.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/mpl/data/test_planner_2d_with_prior_traj.jpg -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/launch/os1-128.yaml: -------------------------------------------------------------------------------- 1 | n_beams: 256 2 | rpm: 300 3 | sample_freq: 18000 4 | min_angle: deg(-30) 5 | max_angle: deg(30) 6 | min_range: 0.0 7 | max_range: 100.0 8 | model: "os1" -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/launch/os1-64.yaml: -------------------------------------------------------------------------------- 1 | n_beams: 64 2 | rpm: 600 3 | sample_freq: 20480 4 | min_angle: deg(-22.5) 5 | max_angle: deg(22.5) 6 | min_range: 0.0 7 | max_range: 50.0 8 | model: "os1" -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/launch/vlp16.yaml: -------------------------------------------------------------------------------- 1 | n_beams: 16 2 | rpm: 300 3 | sample_freq: 18000 4 | min_angle: deg(-15) 5 | max_angle: deg(15) 6 | min_range: 0.0 7 | max_range: 100.0 8 | model: "vlp16" -------------------------------------------------------------------------------- /autonomy_core/control/control_launch/config/mav_manager_params.yaml: -------------------------------------------------------------------------------- 1 | server_wait_timeout: 0.5 2 | need_imu: false 3 | need_output_data: true 4 | use_attitude_safety_catch: false 5 | max_attitude_angle: 0.43 6 | -------------------------------------------------------------------------------- /autonomy_core/control/external-repos.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | kr_mav_control: 3 | type: git 4 | url: https://github.com/KumarRobotics/kr_mav_control 5 | version: fa583db510f534692170b3c885b46d9ab22e664e 6 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mapper/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/data/test_distance_map_planner_2d_iterative.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/mpl/data/test_distance_map_planner_2d_iterative.jpg -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/data/test_distance_map_planner_2d_with_yaw.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/kr_autonomous_flight/HEAD/autonomy_core/map_plan/mpl/data/test_distance_map_planner_2d_with_yaw.jpg -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # TODO: add command line argument for rosmaster uri 4 | docker run -it --rm --network=host --name sim \ 5 | kumarrobotics/autonomy:sim \ 6 | bash 7 | -------------------------------------------------------------------------------- /autonomy_core/base/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # TODO: add command line argument for rosmaster uri 4 | docker run -it --rm --network=host --name autonomy_base_it \ 5 | kumarrobotics/autonomy:base \ 6 | bash -------------------------------------------------------------------------------- /autonomy_core/client/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # TODO: add command line argument for rosmaster uri 4 | docker run -it --rm --network=host --name client \ 5 | kumarrobotics/autonomy:client \ 6 | bash 7 | -------------------------------------------------------------------------------- /autonomy_sim/gazebo_sim/gazebo_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(gazebo_utils) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED) 7 | 8 | catkin_package() 9 | -------------------------------------------------------------------------------- /autonomy_core/control/README.txt: -------------------------------------------------------------------------------- 1 | This module includes necessary config and launch files related to controller. The actual controller implementation is open-sourced in kr_mav_control repository (check external.yaml for detail). -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/action/GoalDistance.action: -------------------------------------------------------------------------------- 1 | # Action goal 2 | kr_tracker_msgs/LineTrackerGoal goal 3 | --- 4 | # Action result 5 | bool at_goal 6 | --- 7 | # Action status 8 | float32 distance_to_goal -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/action/ShortRange.action: -------------------------------------------------------------------------------- 1 | # Action goal 2 | geometry_msgs/PoseStamped goal 3 | int8 auto_mode 4 | --- 5 | # result 6 | bool reach 7 | --- 8 | # status 9 | int32 num_replan 10 | -------------------------------------------------------------------------------- /autonomy_core/control/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # TODO: add command line argument for rosmaster uri 4 | docker run -it --rm --network=host --name autonomy_control_it \ 5 | kumarrobotics/autonomy:control \ 6 | bash 7 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # TODO: add command line argument for rosmaster uri 4 | docker run -it --rm --network=host --name state_machine \ 5 | kumarrobotics/autonomy:state_machine \ 6 | bash 7 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/old-launch/static_tf_vision_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /autonomy_core/interface/px4_interface_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(px4_interface_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 8 | -------------------------------------------------------------------------------- /autonomy_core/client/rqt_quadrotor_safety/scripts/rqt_quadrotor_safety: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | main = Main() 8 | sys.exit(main.main(sys.argv, standalone='rqt_quadrotor_safety.quadrotor_safety.QuadrotorSafety')) 9 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/px4_pluginlists.yaml: -------------------------------------------------------------------------------- 1 | plugin_blacklist: 2 | # common 3 | - safety_area 4 | # extras 5 | - image_pub 6 | - vibration 7 | - distance_sensor 8 | - rangefinder 9 | - wheel_odometry 10 | 11 | 12 | plugin_whitelist: [] 13 | #- 'sys_*' 14 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/README_MSCKF_CALIB_GEN.txt: -------------------------------------------------------------------------------- 1 | 1. make sure your image and imu topics are published correctly 2 | 2. make sure camera and imu frame_id exists in tf tree 3 | 3. run the msckf_calib_gen launch file 4 | 4. result is saved in the dcist_utils/config folder 5 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/src/playground.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "playground"); 5 | 6 | ROS_INFO_STREAM(ros::names::append("", "right")); 7 | ROS_INFO_STREAM(ros::names::append("left", "right")); 8 | 9 | return 0; 10 | } -------------------------------------------------------------------------------- /autonomy_core/interface/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker run -it --rm --network=host --name autonomy_px4_interface_it \ 4 | --privileged \ 5 | --volume="/dev/pixhawk:/dev/pixhawk:rw" \ 6 | --volume="/dev/serial:/dev/serial:rw" \ 7 | kumarrobotics/autonomy:px4_interface \ 8 | bash 9 | 10 | -------------------------------------------------------------------------------- /autonomy_core/client/rqt_quadrotor_safety/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rqt_quadrotor_safety'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/sim/vel_control_for_gain_tuning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /autonomy_core/base/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # setup ros environment 5 | echo "Setting up ros" 6 | echo "source /opt/ros/noetic/setup.bash" >> /$HOME/.bashrc 7 | source /$HOME/.bashrc 8 | 9 | source /opt/ros/noetic/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | -------------------------------------------------------------------------------- /autonomy_core/estimation/fla_ukf/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A 12 state UKF consisting of position, velocity, orientation, accelerometer biases 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/src/map_planner.cpp: -------------------------------------------------------------------------------- 1 | #include "mpl_planner/map_planner.h" 2 | 3 | namespace MPL { 4 | 5 | template 6 | void MapPlanner::setMapUtil( 7 | const std::shared_ptr> &map_util) { 8 | this->env_.reset(new MPL::EnvMap(map_util)); 9 | } 10 | 11 | template class MapPlanner<3>; 12 | 13 | } // namespace MPL 14 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/waypoints/old-waypoints-icra-22-demo.yaml: -------------------------------------------------------------------------------- 1 | - position: [24, -31, 3] # between trees, parallel to the bus area 2 | - position: [7, -11, 3] # between trees, parallel to the bus area 3 | #- position: [52, -41, 3] # above the bus area 4 | - position: [26, -10, 3] # close to home, no tree between this and home 5 | - position: [0, 0, 3] 6 | -------------------------------------------------------------------------------- /autonomy_sim/docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # TODO: add command line argument for rosmaster uri 4 | docker run --gpus all -it --rm --network=host --name state_machine \ 5 | --gpus all \ 6 | --privileged \ 7 | -e DISPLAY="$DISPLAY" \ 8 | -e QT_X11_NO_MITSHM=1 \ 9 | -e XAUTHORITY="$XAUTH" \ 10 | kumarrobotics/autonomy:sim \ 11 | bash 12 | -------------------------------------------------------------------------------- /autonomy_core/interface/mavros_interface/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This reformats an so3_command and publishes it on compatabile mavros topics 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /autonomy_sim/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ -f /root/sim_ws/devel/setup.bash ]; then 4 | echo "Setting up sim" 5 | echo "source /root/sim_ws/devel/setup.bash" >> /"$HOME"/.bashrc 6 | source /"$HOME"/.bashrc 7 | fi 8 | 9 | source /root/sim_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ -f /root/state_machine_ws/devel/setup.bash ]; then 4 | echo "Setting up sim_ws" 5 | echo "source /root/sim_ws/devel/setup.bash" >>/$HOME/.bashrc 6 | source /$HOME/.bashrc 7 | fi 8 | 9 | source /root/sim_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | -------------------------------------------------------------------------------- /autonomy_core/control/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -f /root/control_ws/devel/setup.bash ]; then 4 | echo "Setting up control_ws" 5 | echo "source /root/control_ws/devel/setup.bash" >> /$HOME/.bashrc 6 | source /$HOME/.bashrc 7 | fi 8 | 9 | source /root/control_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/client/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:map_plan 2 | 3 | RUN mkdir -p /root/client_ws/src 4 | WORKDIR /root/client_ws 5 | COPY autonomy_core/client/ src/ 6 | RUN catkin init && catkin config --extend /root/map_plan_ws/devel 7 | RUN catkin build -j2 --no-status -DCMAKE_BUILD_TYPE=Release 8 | 9 | COPY autonomy_core/client/docker/entrypoint.sh /entrypoint.sh 10 | ENTRYPOINT ["/entrypoint.sh"] 11 | -------------------------------------------------------------------------------- /autonomy_core/client/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ -f /root/client_ws/devel/setup.bash ]; then 4 | echo "Setting up client_ws" 5 | echo "source /root/client_ws/devel/setup.bash" >> /$HOME/.bashrc 6 | source /$HOME/.bashrc 7 | fi 8 | 9 | source /root/client_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/map_plan_launch/launch/planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /autonomy_core/client/docker/deploy2.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | xhost +local:root 4 | docker run --gpus all -it \ 5 | -e DISPLAY \ 6 | -e QT_X11_NO_MITSHM=1 \ 7 | --env="DISPLAY=$DISPLAY" \ 8 | -v "/tmp/.X11-unix:/tmp/.X11-unix" \ 9 | --rm \ 10 | --privileged \ 11 | --network=host \ 12 | --name client \ 13 | kumarrobotics/autonomy:client \ 14 | roslaunch client_launch client.launch 15 | -------------------------------------------------------------------------------- /autonomy_core/estimation/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -f /root/estimation_ws/devel/setup.bash ]; then 4 | echo "Setting up estimation_ws" 5 | echo "source /root/estimation_ws/devel/setup.bash" >> /$HOME/.bashrc 6 | source /$HOME/.bashrc 7 | fi 8 | 9 | source /root/estimation_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ -f /root/map_plan_ws/devel/setup.bash ]; then 4 | echo "Setting up map_plan_ws" 5 | echo "source /root/map_plan_ws/devel/setup.bash" >> /$HOME/.bashrc 6 | source /$HOME/.bashrc 7 | fi 8 | 9 | source /root/map_plan_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | 17 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/apriltag.yaml: -------------------------------------------------------------------------------- 1 | target_type: 'aprilgrid' #gridtype 2 | tagCols: 7 #number of apriltags 3 | tagRows: 5 #number of apriltags 4 | tagSize: 0.04 #size of apriltag, edge to edge [m] 5 | tagSpacing: 0.25 #ratio of space between tags to tagSize 6 | #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-] 7 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/so3_control_gains.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: {x: 5.0, y: 5.0, z: 5.8} 3 | vel: {x: 2.8, y: 2.8, z: 4.0} 4 | ki: {x: 0.01, y: 0.01, z: 0.1} 5 | kib: {x: 0.00, y: 0.00, z: 0.00} 6 | rot: {x: 1.5, y: 1.5, z: 1.0} 7 | ang: {x: 0.13, y: 0.13, z: 0.05} 8 | 9 | corrections: 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.5 15 | max_pos_int_b: 0.5 16 | -------------------------------------------------------------------------------- /autonomy_core/control/control_launch/config/so3_control_gains.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: { x: 4.0, y: 4.0, z: 4.0 } 3 | vel: { x: 3.5, y: 3.5, z: 3.5 } 4 | ki: { x: 0.00, y: 0.00, z: 0.00 } 5 | kib: { x: 0.00, y: 0.00, z: 0.00 } 6 | rot: { x: 8.0, y: 8.0, z: 6.0 } 7 | ang: { x: 5.0, y: 5.0, z: 5.0} 8 | 9 | corrections: 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.3 15 | max_pos_int_b: 0.3 16 | -------------------------------------------------------------------------------- /autonomy_core/control/control_launch/config/trackers_mp.yaml: -------------------------------------------------------------------------------- 1 | trackers: 2 | - kr_trackers/LineTrackerMinJerk 3 | - kr_trackers/LineTrackerDistance 4 | - kr_trackers/VelocityTracker 5 | - kr_trackers/NullTracker 6 | - action_trackers/TakeOffTracker 7 | - action_trackers/LandTracker 8 | - action_trackers/StoppingPolicy 9 | - action_trackers/ActionTrajectoryTracker 10 | #- action_trackers/ActionPathTracker 11 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/action/RunPath.action: -------------------------------------------------------------------------------- 1 | # Action goal 2 | kr_planning_msgs/Path path 3 | int64 epoch 4 | float32 local_replan_rate 5 | int16 global_replan_rate_factor 6 | bool align_yaw 7 | bool block # will not return until traj is complete (cannot be used with replanning) 8 | --- 9 | # Action result 10 | bool at_goal 11 | --- 12 | # Action status 13 | float32 time_remaining 14 | float32 percent_complete 15 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/action/RunTrajectory.action: -------------------------------------------------------------------------------- 1 | # Action goal 2 | kr_planning_msgs/SplineTrajectory traj 3 | int64 epoch 4 | float32 local_replan_rate 5 | int16 global_replan_rate_factor 6 | bool block # will not return until traj is complete (cannot be used with replanning) 7 | --- 8 | # Action result 9 | bool at_goal 10 | --- 11 | # Action status 12 | float32 time_remaining 13 | float32 percent_complete 14 | -------------------------------------------------------------------------------- /autonomy_core/interface/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ -f /root/px4_interface_ws/devel/setup.bash ]; then 4 | echo "Setting up px4_interface_ws" 5 | echo "source /root/px4_interface_ws/devel/setup.bash" >> /$HOME/.bashrc 6 | source /$HOME/.bashrc 7 | fi 8 | 9 | source /root/px4_interface_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ -f /root/state_machine_ws/devel/setup.bash ]; then 4 | echo "Setting up state_machine_ws" 5 | echo "source /root/state_machine_ws/devel/setup.bash" >> /$HOME/.bashrc 6 | source /$HOME/.bashrc 7 | fi 8 | 9 | source /root/state_machine_ws/devel/setup.bash 10 | 11 | if [ "$#" -eq 0 ]; then 12 | exec bash 13 | else 14 | exec "$@" 15 | fi 16 | 17 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:state_machine 2 | 3 | RUN mkdir -p /root/sim_ws/src 4 | WORKDIR /root/sim_ws 5 | COPY pkgs src/pkgs 6 | RUN rm src/pkgs/rosflight_msgs/CATKIN_IGNORE 7 | RUN catkin init && catkin config --extend /root/state_machine_ws/devel \ 8 | && catkin build -j8 -DCMAKE_BUILD_TYPE=RelWithDebInfo 9 | 10 | COPY docker/entrypoint.sh /entrypoint.sh 11 | ENTRYPOINT ["/entrypoint.sh"] 12 | 13 | -------------------------------------------------------------------------------- /external_real_robot.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | # remember to remove other versions of ouster_example repo first 3 | ouster_example: 4 | type: git 5 | url: http://github.com/ouster-lidar/ouster_example.git 6 | version: 43107a1d14b8cd22378a830bd07aa7acca1269a4 7 | 8 | ouster_decoder: 9 | type: git 10 | url: http://github.com/KumarRobotics/ouster_decoder.git 11 | version: d66b52d9941647f459c31f81a344c6e9dc77ab34 12 | -------------------------------------------------------------------------------- /autonomy_core/estimation/fla_ukf/config/params.yaml: -------------------------------------------------------------------------------- 1 | alpha: 0.1 2 | beta: 2.0 3 | kappa: 0.0 4 | 5 | noise_std: 6 | process: 7 | acc: 8 | x: 5.0 9 | y: 5.0 10 | z: 5.0 11 | gyro: 12 | x: 0.50 13 | y: 0.50 14 | z: 0.50 15 | acc_bias: 16 | x: 0.01 17 | y: 0.01 18 | z: 0.01 19 | gyro_bias: 20 | x: 0.01 21 | y: 0.01 22 | z: 0.01 23 | vio_yaw_drift: 0.01 24 | -------------------------------------------------------------------------------- /autonomy_sim/gazebo_sim/gazebo_utils/config/falcon4_os1_so3_gains.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: {x: 2.0, y: 2.0, z: 3.2} 3 | vel: {x: 1.7, y: 1.7, z: 3.0} 4 | ki: {x: 0.00, y: 0.00, z: 0.00} 5 | kib: {x: 0.00, y: 0.00, z: 0.00} 6 | rot: {x: 1.5, y: 1.5, z: 1.3} 7 | ang: {x: 0.2, y: 0.2, z: 0.10} 8 | 9 | corrections: 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.5 15 | max_pos_int_b: 0.5 16 | max_tilt_angle: 3.14 17 | -------------------------------------------------------------------------------- /external_coverage_planner.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | 3 | polygon_coverage_planning: 4 | type: git 5 | url: http://github.com/XuRobotics/polygon_coverage_planning.git 6 | version: master 7 | 8 | cgal_catkin: 9 | type: git 10 | url: http://github.com/ethz-asl/cgal_catkin.git 11 | version: master 12 | 13 | catkin_simple: 14 | type: git 15 | url: http://github.com/catkin/catkin_simple.git 16 | version: master 17 | -------------------------------------------------------------------------------- /autonomy_core/estimation/estimation_launch/config/ukf_params.yaml: -------------------------------------------------------------------------------- 1 | alpha: 0.1 2 | beta: 2.0 3 | kappa: 0.0 4 | 5 | noise_std: 6 | process: 7 | acc: 8 | x: 5.0 9 | y: 5.0 10 | z: 5.0 11 | gyro: 12 | x: 0.50 13 | y: 0.50 14 | z: 0.50 15 | acc_bias: 16 | x: 0.01 17 | y: 0.01 18 | z: 0.01 19 | gyro_bias: 20 | x: 0.01 21 | y: 0.01 22 | z: 0.01 23 | vio_yaw_drift: 0.01 24 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/so3_gains_backup/so3_control_gains_nov_4_before_tuning.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: {x: 4.3, y: 4.3, z: 5.8} 3 | vel: {x: 2.8, y: 2.8, z: 4.0} 4 | ki: {x: 0.01, y: 0.01, z: 0.1} 5 | kib: {x: 0.00, y: 0.00, z: 0.00} 6 | rot: {x: 1.5, y: 1.5, z: 1.0} 7 | ang: {x: 0.13, y: 0.13, z: 0.05} 8 | 9 | corrections: 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.5 15 | max_pos_int_b: 0.5 16 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jps3d 4 | 1.0.0 5 | The jps3d package 6 | sikang 7 | 8 | BSD-3-Clause 9 | catkin 10 | 11 | kr_planning_msgs 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /.github/workflows/pylint-reviewdog.yaml: -------------------------------------------------------------------------------- 1 | name: pylint-reviewdog 2 | on: [pull_request] 3 | 4 | jobs: 5 | pylint: 6 | name: runner / pylint 7 | runs-on: ubuntu-latest 8 | steps: 9 | - uses: actions/checkout@v2 10 | - uses: dciborow/action-pylint@0.0.4 11 | with: 12 | github_token: ${{ secrets.KR_AUTONOMOUS_FLIGHT_TOKEN_REVIEWDOG }} 13 | reporter: github-pr-check 14 | level: error 15 | pylint_rc: '.pylintrc-reviewdog' 16 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | View trajectory messages in rviz natively!! 7 | Currently supports Mike's style and Sikang's style 8 | Defaults to the better style :P 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/so3_gains_backup/so3_control_gains_oct_24_before_tuning.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: { x: 4.3, y: 4.3, z: 5.8 } 3 | vel: { x: 2.8, y: 2.8, z: 4.0 } 4 | ki: { x: 0.01, y: 0.01, z: 0.1 } 5 | kib: { x: 0.00, y: 0.00, z: 0.00 } 6 | rot: { x: 1.5, y: 1.5, z: 1.0 } 7 | ang: { x: 0.13, y: 0.13, z: 0.05 } 8 | 9 | corrections: 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.5 15 | max_pos_int_b: 0.5 16 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/config/overpasscity_demo.yaml: -------------------------------------------------------------------------------- 1 | unity_command: 2 | initial_commands: [ 3 | '$(arg unity_namespace)Settings set_frame_rate 30', 4 | '$(arg unity_namespace)Clock realtime false', 5 | '$(arg unity_namespace)Spawner load $(find lejeune_emout)/unity/overpasscity_cardetection', 6 | '$(arg unity_namespace)Spawner scene OverpassCity', 7 | '$(arg unity_namespace)Settings configure GuaranteeFrameRate:true' 8 | ] 9 | 10 | use_sim_time: true 11 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/relay_ovc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /autonomy_core/client/rqt_quadrotor_safety/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(rqt_quadrotor_safety) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_python_setup() 6 | 7 | catkin_package() 8 | 9 | install(FILES plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | 11 | install(DIRECTORY resource DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 12 | 13 | install(PROGRAMS scripts/rqt_quadrotor_safety 14 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 15 | -------------------------------------------------------------------------------- /autonomy_sim/external.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | mrsl_quadrotor: 3 | type: git 4 | url: https://github.com/KumarRobotics/mrsl_quadrotor 5 | version: master 6 | 7 | ouster_example: 8 | type: git 9 | url: http://github.com/XuRobotics/ouster_example.git 10 | version: os-sim 11 | 12 | # TODO: use compose to avoid getting this here 13 | waypoint_navigation_plugin: 14 | type: git 15 | url: https://github.com/KumarRobotics/waypoint_navigation_plugin.git 16 | version: master 17 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dcist_utils 4 | 0.0.0 5 | The dcist_utils package 6 | 7 | Chao Qu 8 | Xu Liu 9 | 10 | Penn Software License 11 | 12 | catkin 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /.github/workflows/shellcheck-reviewdog.yaml: -------------------------------------------------------------------------------- 1 | name: shellcheck-reviewdog 2 | on: [pull_request] 3 | 4 | jobs: 5 | shellcheck: 6 | runs-on: ubuntu-latest 7 | steps: 8 | - uses: actions/checkout@v1 9 | - uses: reviewdog/action-shellcheck@master 10 | with: 11 | github_token: ${{ secrets.KR_AUTONOMOUS_FLIGHT_TOKEN_REVIEWDOG }} 12 | reporter: github-pr-check 13 | exclude: | 14 | ./.git/* 15 | autonomy_sim/docker/entrypoint.sh 16 | autonomy_sim/docker/run.sh 17 | -------------------------------------------------------------------------------- /autonomy_core/control/control_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control_launch 4 | 0.0.0 5 | The control_launch package 6 | 7 | Xu Liu 8 | Chao Qu 9 | 10 | Penn Software License 11 | catkin 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /autonomy_core/estimation/estimation_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | estimation_launch 4 | 0.0.0 5 | The estimation_launch package 6 | 7 | Xu Liu 8 | Chao Qu 9 | 10 | Penn Software License 11 | catkin 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | coverage_utils 4 | 0.0.1 5 | TODO 6 | 7 | Xu Liu 8 | 9 | Penn Software License 10 | 11 | 12 | roscpp 13 | kr_planning_msgs 14 | 15 | 16 | catkin 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/wercker.yml: -------------------------------------------------------------------------------- 1 | box: ubuntu 2 | 3 | build: 4 | steps: 5 | - script: 6 | name: install dependencies 7 | code: | 8 | sudo apt-get update 9 | sudo apt install -y libeigen3-dev libyaml-cpp-dev libboost-dev cmake 10 | - script: 11 | name: build 12 | code: | 13 | mkdir build 14 | cd build 15 | cmake .. 16 | make -j4 17 | - script: 18 | name: test 19 | code: | 20 | cd build 21 | make test 22 | 23 | 24 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_sloam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fake_sloam 4 | 0.0.0 5 | The fake_sloam package 6 | 7 | Xu Liu 8 | Chao Qu 9 | Guilherme Nardari 10 | Penn Software License 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /autonomy_core/interface/px4_interface_launch/README.txt: -------------------------------------------------------------------------------- 1 | Only used in real-robot experiments. 2 | You will need to update the following propeller parameters in SO3_command_to_mavros.launch according to the following equation: 3 | Thrust = a * RPM^2 + b * RPM + c (Thrust is in Newton not grams-force) 4 | 5 | 6 | 7 | Contact the maintainers if you are not sure how to calculate those parameters. -------------------------------------------------------------------------------- /autonomy_core/interface/px4_interface_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | px4_interface_launch 4 | 0.0.0 5 | The px4_interface_launch package 6 | 7 | chao 8 | steven 9 | 10 | Penn Software License 11 | catkin 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/script/optimal_acc_primitive.py: -------------------------------------------------------------------------------- 1 | from sympy import Symbol, Matrix, simplify, diff 2 | 3 | dp = Symbol('dp')#dp = pf -p0 4 | 5 | vf = Symbol('vf') 6 | v0 = Symbol('v0') 7 | 8 | T = Symbol('T') 9 | 10 | A = Matrix([[-12/T**3, 6/T**2], [6/T**2, -2/T]]) 11 | b = Matrix([dp-v0*T, vf-v0]) 12 | 13 | d = simplify(A*b) 14 | 15 | d3 = d[0] 16 | d2 = d[1] 17 | 18 | print(d3) 19 | print(d2) 20 | 21 | C = T**3/3*d3*d3 + T*d2*d2+d3*d2*T**2 22 | C = simplify(C) 23 | 24 | print("C = ", C) 25 | print("C' = ", diff(C, T, 1)) 26 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | state_machine_launch 4 | 0.0.0 5 | The state_machine_launch package 6 | 7 | Xu Liu 8 | Chao Qu 9 | 10 | Penn Software License 11 | catkin 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/include/coverage_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | class CoveragePlanner { 10 | public: 11 | CoveragePlanner(); 12 | void RunPlanner(); 13 | 14 | private: 15 | ros::NodeHandle nh_; 16 | ros::NodeHandle pnh_; 17 | ros::Publisher path_pub_; 18 | ros::Publisher all_points_pub_; 19 | ros::Publisher initial_polygon_publisher_; 20 | std::string fname_; 21 | }; 22 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/imu_for_kalibr.yaml: -------------------------------------------------------------------------------- 1 | #Accelerometers 2 | accelerometer_noise_density: 1.0e-03 #Noise density (continuous-time) 3 | accelerometer_random_walk: 1.0e-04 #Bias random walk 4 | 5 | #Gyroscopes 6 | gyroscope_noise_density: 1.0e-04 #Noise density (continuous-time) 7 | gyroscope_random_walk: 1.0e-05 #Bias random walk 8 | 9 | rostopic: /ovc/vectornav/imu #the IMU ROS topic 10 | update_rate: 400.0 #Hz (for discretization of the values above) 11 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/kalibr-8-30-2021/result-1st-success-refined-reproj-error-0.5/imu-.calibration-8-30-2021cam-imu.yaml: -------------------------------------------------------------------------------- 1 | imu0: 2 | T_i_b: 3 | - [1.0, 0.0, 0.0, 0.0] 4 | - [0.0, 1.0, 0.0, 0.0] 5 | - [0.0, 0.0, 1.0, 0.0] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | accelerometer_noise_density: 0.001 8 | accelerometer_random_walk: 0.0001 9 | gyroscope_noise_density: 0.0001 10 | gyroscope_random_walk: 1.0e-05 11 | model: calibrated 12 | rostopic: /ovc/vectornav/imu 13 | time_offset: 0.0 14 | update_rate: 400.0 15 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | motion_primitive_library 3 | 1.2.0 4 | search-based motion primitive planning library 5 | Sikang Liu 6 | Sikang Liu 7 | Apache-2.0 8 | 9 | catkin 10 | 11 | kr_planning_msgs 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_launch/launch/replanner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /autonomy_core/interface/mavros_interface/params/extras.txt: -------------------------------------------------------------------------------- 1 | mavlink stop-all 2 | mavlink start -r 800000 -d /dev/ttyACM0 -m config -x 3 | mavlink start -d /dev/ttyS2 -b 921600 -m custom -r 90000 -x 4 | mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 100 5 | mavlink stream -d /dev/ttyS2 -s HIGHRES_IMU -r 100 6 | mavlink stream -d /dev/ttyS2 -s RC_CHANNELS -r 10 7 | mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 10 8 | mavlink stream -d /dev/ttyS2 -s DISTANCE_SENSOR -r 100 9 | mavlink stream -d /dev/ttyS2 -s SYS_STATUS -r 10 10 | mavlink stream -d /dev/ttyS2 -s GPS_RAW_INT -r 10 11 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:map_plan 2 | 3 | RUN apt-get update \ 4 | && apt-get install -y \ 5 | python3-yaml 6 | 7 | RUN pip3 install rospkg 8 | 9 | RUN mkdir -p /root/state_machine_ws/src 10 | WORKDIR /root/state_machine_ws 11 | COPY autonomy_core/state_machine/ src/ 12 | RUN catkin init && catkin config --extend /root/map_plan_ws/devel 13 | RUN catkin build -j2 --no-status -DCMAKE_BUILD_TYPE=RelWithDebInfo 14 | 15 | COPY autonomy_core/state_machine/docker/entrypoint.sh /entrypoint.sh 16 | ENTRYPOINT ["/entrypoint.sh"] 17 | 18 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/wercker.yml: -------------------------------------------------------------------------------- 1 | box: ubuntu 2 | 3 | build: 4 | steps: 5 | - script: 6 | name: install dependencies 7 | code: | 8 | export DEBIAN_FRONTEND=noninteractive 9 | sudo apt-get update 10 | sudo apt install -y libeigen3-dev libyaml-cpp-dev libproj-dev libopencv-dev cmake 11 | - script: 12 | name: build 13 | code: | 14 | mkdir build 15 | cd build 16 | cmake .. 17 | make -j4 18 | - script: 19 | name: test 20 | code: | 21 | cd build 22 | make test 23 | 24 | 25 | -------------------------------------------------------------------------------- /autonomy_core/client/client_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | client_launch 4 | 0.0.0 5 | The client_launch package 6 | 7 | Xu Liu 8 | Fernando Cladera 9 | Chao Qu 10 | 11 | Penn Software License 12 | catkin 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /autonomy_sim/gazebo_sim/gazebo_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gazebo_utils 4 | 0.0.0 5 | The gazebo_utils package 6 | 7 | Xu Liu 8 | Chao Qu 9 | Guilherme Nardari 10 | 11 | Penn Software License 12 | 13 | catkin 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | traj_opt_ros 4 | 0.0.1 5 | The traj_opt_ros package 6 | 7 | Mike Watterson 8 | 9 | Penn Software License 10 | 11 | catkin 12 | 13 | roscpp 14 | kr_planning_msgs 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/waypoints/waypoints-new-icra-22-demo.yaml: -------------------------------------------------------------------------------- 1 | - position: [6, -27, 3] # on the road 2 | - position: [26, -38, 3] # 3 | - position: [27, -22, 3] # back in the firehygrant` 4 | - position: [0, 0, 3] 5 | - position: [6, -27, 3] # on the road 6 | - position: [26, -38, 3] # 7 | - position: [27, -22, 3] # back in the firehygrant` 8 | - position: [0, 0, 3] 9 | - position: [6, -27, 3] # on the road 10 | - position: [26, -38, 3] # 11 | - position: [27, -22, 3] # back in the firehygrant` 12 | - position: [0, 0, 3] 13 | # - position: [31, -7, 3] # back in the firehygrant` 14 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/old-launch/test_mapper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /autonomy_core/estimation/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:base 2 | 3 | RUN apt-get update \ 4 | && apt-get install -y \ 5 | ros-noetic-random-numbers \ 6 | libsuitesparse-dev 7 | 8 | RUN mkdir -p /root/estimation_ws/src 9 | WORKDIR /root/estimation_ws 10 | 11 | COPY autonomy_core/estimation/ src/ 12 | RUN catkin init && catkin config --extend /opt/ros/noetic 13 | RUN vcs import --input src/external-repos.yaml src/ 14 | RUN catkin build -j2 --no-status -DCMAKE_BUILD_TYPE=Release 15 | 16 | COPY autonomy_core/estimation/docker/entrypoint.sh /entrypoint.sh 17 | ENTRYPOINT ["/entrypoint.sh"] 18 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/script/takeoff.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ $# -eq 0 ]; then 4 | ROBOT="quadrotor" 5 | else 6 | ROBOT=$robot 7 | fi 8 | echo "robot name ${ROBOT}" 9 | 10 | # echo "Enable motors..." 11 | # rosservice call /$ROBOT/mav_services/motors true 12 | # sleep 1 13 | 14 | # echo "Takeoff..." 15 | # rosservice call /$ROBOT/mav_services/takeoff 16 | # sleep 1 17 | 18 | rosrun arl_unity_ros_air rosflight_offboard.py __ns:=${ROBOT} 19 | 20 | # read -p "Press [Enter] to go to [1, 1, 1]" 21 | # rosservice call /$ROBOT/mav_services/goTo "goal: [1.0, 1.0, 1.0, 0.0]" 22 | # sleep 1 23 | -------------------------------------------------------------------------------- /autonomy_core/interface/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:control 2 | 3 | RUN mkdir -p /root/px4_interface_ws/src 4 | WORKDIR /root/px4_interface_ws 5 | COPY pkgs src/pkgs 6 | COPY scripts scripts 7 | 8 | RUN catkin init && catkin config --extend /root/control_ws/devel \ 9 | && catkin build -j4 -DCMAKE_BUILD_TYPE=RelWithDebInfo 10 | 11 | WORKDIR scripts 12 | RUN chmod +x install_geographiclib_datasets.sh \ 13 | && ./install_geographiclib_datasets.sh 14 | 15 | RUN usermod -aG dialout $(whoami) 16 | 17 | COPY autonomy_core/docker/entrypoint.sh /entrypoint.sh 18 | ENTRYPOINT ["/entrypoint.sh"] 19 | 20 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/map_plan_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | map_plan_launch 4 | 0.0.0 5 | The mapper_planner_launch package 6 | 7 | Xu Liu 8 | Chao Qu 9 | Guilherme Nardari 10 | 11 | Penn Software License 12 | catkin 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | real_experiment_launch 4 | 0.0.0 5 | The real_experiment_launch package 6 | 7 | Xu Liu 8 | Fernando Cladera 9 | Chao Qu 10 | 11 | Penn Software License 12 | 13 | catkin 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /autonomy_core/control/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:base 2 | 3 | RUN apt-get update \ 4 | && apt-get install -y \ 5 | ros-noetic-mavros \ 6 | ros-noetic-mavros-msgs \ 7 | ros-noetic-mavros-extras 8 | 9 | RUN mkdir -p /root/control_ws/src 10 | WORKDIR /root/control_ws 11 | COPY autonomy_core/control/ src/ 12 | RUN catkin init && catkin config --extend /opt/ros/noetic 13 | RUN vcs import --input src/external-repos.yaml src/ 14 | RUN catkin build -j2 --no-status -DCMAKE_BUILD_TYPE=Release 15 | 16 | COPY autonomy_core/control/docker/entrypoint.sh /entrypoint.sh 17 | ENTRYPOINT ["/entrypoint.sh"] 18 | 19 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/old-launch/record_bag_for_GUI.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 23 | 24 | -------------------------------------------------------------------------------- /autonomy_sim/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:state_machine 2 | 3 | RUN apt update \ 4 | && apt install -y \ 5 | ros-noetic-gazebo-ros \ 6 | ros-noetic-hector-gazebo 7 | 8 | RUN mkdir -p /root/sim_ws/src 9 | WORKDIR /root/sim_ws 10 | COPY autonomy_sim/gazebo_sim/ src/ 11 | COPY autonomy_sim/external.yaml src/ 12 | COPY autonomy_core/client/ src/ 13 | RUN catkin init && catkin config --extend /root/state_machine_ws/devel 14 | RUN vcs import --input src/external.yaml src/ 15 | RUN catkin build -j2 --no-status -DCMAKE_BUILD_TYPE=RelWithDebInfo 16 | 17 | COPY autonomy_sim/docker/entrypoint.sh /entrypoint.sh 18 | ENTRYPOINT ["/entrypoint.sh"] 19 | 20 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_launch/launch/state_machine.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/rosflight_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/src/coverage_utils_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "coverage_planner"); 5 | ros::NodeHandle nh; 6 | CoveragePlanner planner; 7 | ros::Rate r(0.5); 8 | // while (nh.ok()) { 9 | r.sleep(); 10 | planner.RunPlanner(); 11 | ros::spinOnce(); 12 | ROS_INFO("Publishing data for coverage..."); 13 | ROS_INFO( 14 | "You can visualize data in RVIZ by subscribing to /all_input_points " 15 | "topic, and change the fixed frame to map. The topic has the type: " 16 | "planning_ros_utils/Path (open RVIZ before launching this node)."); 17 | // } 18 | } -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(jps3d) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED 7 | COMPONENTS kr_planning_msgs) 8 | find_package(Eigen3 REQUIRED) 9 | find_package(Boost REQUIRED) 10 | 11 | catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS kr_planning_msgs) 12 | 13 | add_library(${PROJECT_NAME} src/graph_search.cpp src/jps_planner.cpp 14 | src/map_util.cpp) 15 | target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) 16 | target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen Boost::boost) 17 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/script/optimal_jerk_primitive.py: -------------------------------------------------------------------------------- 1 | from sympy import Symbol, Matrix, simplify, diff 2 | 3 | dp = Symbol('dp')#dp = pf -p0 4 | 5 | vf = Symbol('vf') 6 | v0 = Symbol('v0') 7 | 8 | af = Symbol('af') 9 | a0 = Symbol('a0') 10 | 11 | T = Symbol('T') 12 | 13 | A = Matrix([[720, -360*T, 60*T*T],[-360*T, 168*T*T, -24*T*T*T], [60*T*T, -24*T*T*T, 3*T*T*T*T]]) 14 | b = Matrix([dp-v0*T-0.5*a0*T*T, vf-v0-a0*T, af-a0]) 15 | 16 | d = simplify(A*b/T**5) 17 | 18 | d5 = d[0] 19 | d4 = d[1] 20 | d3 = d[2] 21 | 22 | C = T**5/20*d5*d5 + T**3/3*d4*d4+T*d3*d3+T**4/4*d4*d5+T**3/3*d3*d5+T*T*d3*d4 23 | C = simplify(C) 24 | 25 | print("C = ", C) 26 | print("C' = ", diff(C, T, 1)) 27 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/launch/coverage_utils.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/trackers.yaml: -------------------------------------------------------------------------------- 1 | trackers: 2 | - std_trackers/LineTrackerMinJerkAction 3 | - std_trackers/LineTrackerDistanceAction 4 | # - std_trackers/LineTrackerTrapezoid 5 | # - std_trackers/LineTrackerYaw 6 | - std_trackers/VelocityTracker 7 | - std_trackers/NullTracker 8 | # - std_trackers/CircleTrackerAction 9 | # - std_trackers/TrajectoryTracker 10 | # - std_trackers/SmoothVelTrackerAction 11 | # - std_trackers/LissajousTrackerAction 12 | # - std_trackers/LissajousAdderAction 13 | - action_trackers/TakeOffTracker 14 | - action_trackers/LandTracker 15 | - action_trackers/ActionPathTracker 16 | - action_trackers/StoppingPolicy 17 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mapper/include/mapper/tf_listener.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace mapper { 10 | 11 | class TFListener { 12 | public: 13 | TFListener() : listener_(buffer_) {} 14 | 15 | std::optional LookupTransform(const std::string& target, 16 | const std::string& source, 17 | const ros::Time& time); 18 | 19 | private: 20 | tf2_ros::Buffer buffer_; 21 | tf2_ros::TransformListener listener_; 22 | }; 23 | 24 | } // namespace mapper 25 | -------------------------------------------------------------------------------- /docs/ouster_metadata/parse_ouster_metadata_from_rosbag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # modified based on http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29 3 | import rospy 4 | from std_msgs.msg import String 5 | import json 6 | 7 | def callback(data): 8 | x = json.loads(data.data) 9 | with open('ouster_metadata_parsed.json', 'w') as f: 10 | # Dump metadata into json 11 | json.dump(x, f, ensure_ascii=False, indent=4) 12 | print("metadata saved") 13 | 14 | def listener(): 15 | rospy.init_node('parse_ouster_metadata', anonymous=True) 16 | rospy.Subscriber("/os_node/metadata", String, callback) 17 | rospy.spin() 18 | 19 | if __name__ == '__main__': 20 | listener() 21 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/include/mpl_planner/map_planner.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file map_planner.h 3 | * @brief motion planning using voxel map for collision checking 4 | */ 5 | 6 | #pragma once 7 | 8 | #include "mpl_planner/env_map.h" 9 | #include "mpl_planner/planner_base.h" 10 | 11 | namespace MPL { 12 | 13 | template 14 | class MapPlanner final : public PlannerBase { 15 | public: 16 | MapPlanner(bool verbose) : PlannerBase(verbose) {} 17 | 18 | /// Set map util 19 | void setMapUtil(const std::shared_ptr>& map_util); 20 | }; 21 | 22 | /// Planner for 2D OccMap 23 | typedef MapPlanner<2> OccMapPlanner; 24 | /// Planner for 3D VoxelMap 25 | typedef MapPlanner<3> VoxelMapPlanner; 26 | 27 | } // namespace MPL 28 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/include/mpl_basis/control.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file control.h 3 | * @brief Control classes 4 | */ 5 | #pragma once 6 | 7 | namespace MPL { 8 | 9 | /// Enum for control input 10 | enum Control { 11 | NONE = 0, ///< default uninitialized value 12 | VEL = 0b00001, ///< control input is vel 13 | ACC = 0b00011, ///< control input is acc 14 | JRK = 0b00111, ///< control input is jrk 15 | SNP = 0b01111, ///< control input is snp 16 | VELxYAW = 0b10001, ///< control input is vel and yaw 17 | ACCxYAW = 0b10011, ///< control input is acc and yaw 18 | JRKxYAW = 0b10111, ///< control input is jrk and yaw 19 | SNPxYAW = 0b11111 ///< control input is snp and yaw 20 | }; 21 | 22 | } // namespace MPL 23 | -------------------------------------------------------------------------------- /autonomy_core/client/rqt_quadrotor_safety/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A Python GUI plugin to steer a quadrotor with a geometry_msgs/Twist message. 5 | 6 | 7 | 8 | 9 | folder 10 | Plugins related to quadrotor tools. 11 | 12 | 13 | nm-signal-25 14 | A Python GUI plugin for controlling the FLA platform 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/msckf_calib_gen.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/estimation/fla_ukf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fla_ukf 4 | 0.0.0 5 | The fla_ukf package 6 | 7 | Kartik Mohta 8 | Penn Software License 9 | 10 | catkin 11 | 12 | cmake_modules 13 | 14 | eigen_conversions 15 | nav_msgs 16 | nodelet 17 | roscpp 18 | sensor_msgs 19 | tf2_ros 20 | angles 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(traj_opt_ros) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp kr_planning_msgs) 7 | find_package(Eigen3 REQUIRED NO_MODULE) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS include 11 | LIBRARIES ${PROJECT_NAME} 12 | CATKIN_DEPENDS kr_planning_msgs 13 | DEPENDS EIGEN3) 14 | 15 | add_library(${PROJECT_NAME} src/ros_bridge.cpp src/msg_traj.cpp 16 | src/polynomial_basis.cpp src/trajectory.cpp) 17 | target_include_directories(${PROJECT_NAME} PUBLIC include 18 | ${catkin_INCLUDE_DIRS}) 19 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) 20 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/data/simple3d.yaml: -------------------------------------------------------------------------------- 1 | - start: [0, 0, 0] 2 | - goal: [4, 4, 4] 3 | - origin: [0, 0, 0] 4 | - dim: [5, 5, 5] 5 | - resolution: 1 6 | 7 | # Simple 5x5x5 gird 8 | - data: [ 9 | # ↓ start 10 | 0, 1, 1, 1, 1, 11 | 0, 1, 1, 1, 1, 12 | 0, 1, 1, 1, 1, 13 | 0, 1, 1, 1, 1, 14 | 0, 1, 1, 1, 1, 15 | 16 | 1, 1, 1, 1, 1, 17 | 1, 1, 1, 1, 1, 18 | 1, 1, 1, 1, 1, 19 | 1, 1, 1, 1, 1, 20 | 0, 0, 0, 0, 0, 21 | 22 | 1, 1, 1, 1, 0, 23 | 1, 1, 1, 1, 0, 24 | 1, 1, 1, 1, 0, 25 | 1, 1, 1, 1, 0, 26 | 1, 1, 1, 1, 0, 27 | 28 | 0, 0, 0, 0, 0, 29 | 1, 1, 1, 1, 1, 30 | 1, 1, 1, 1, 1, 31 | 1, 1, 1, 1, 1, 32 | 1, 1, 1, 1, 1, 33 | 34 | 0, 1, 1, 1, 1, 35 | 0, 1, 1, 1, 1, 36 | 0, 0, 0, 1, 1, 37 | 0, 1, 1, 0, 1, 38 | 0, 1, 1, 1, 0, # ← goal 39 | ] 40 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM kumarrobotics/autonomy:control 2 | 3 | RUN apt-get update && \ 4 | apt-get install -y \ 5 | ros-noetic-image-pipeline \ 6 | ros-noetic-image-geometry \ 7 | libqt5widgets5 \ 8 | libnlopt-dev \ 9 | libsdl-dev \ 10 | libsdl-image1.2-dev \ 11 | ros-noetic-image-geometry \ 12 | ros-noetic-depth-image-proc 13 | 14 | RUN mkdir -p /root/map_plan_ws/src 15 | WORKDIR /root/map_plan_ws 16 | COPY autonomy_core/map_plan/ src/ 17 | 18 | RUN catkin init && catkin config --extend /root/control_ws/devel 19 | RUN vcs import --input src/external-repos.yaml src/ 20 | RUN catkin build -j2 --no-status -DCMAKE_BUILD_TYPE=RelWithDebInfo 21 | 22 | COPY autonomy_core/map_plan/docker/entrypoint.sh /entrypoint.sh 23 | ENTRYPOINT ["/entrypoint.sh"] 24 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/jps3d/include/jps/data_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file data_utils.h 3 | * @brief Provide a few widely used function for basic type 4 | */ 5 | 6 | #pragma once 7 | 8 | #include "jps/data_type.h" 9 | 10 | namespace JPS { 11 | 12 | /// Template for transforming a vector 13 | template 14 | vec_E transform_vec(const vec_E &t, const TF &tf) { 15 | vec_E new_t; 16 | for (const auto &it : t) new_t.push_back(tf * it); 17 | return new_t; 18 | } 19 | 20 | /// Template for calculating distance 21 | template 22 | decimal_t total_distance(const vec_E &vs) { 23 | decimal_t dist = 0; 24 | for (unsigned int i = 1; i < vs.size(); i++) 25 | dist += (vs[i] - vs[i - 1]).norm(); 26 | 27 | return dist; 28 | } 29 | 30 | } // namespace JPS 31 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/config/emu_stereo_lidar_old_format.yaml: -------------------------------------------------------------------------------- 1 | commands: 2 | - "$(arg unity_namespace)Spawner spawn Quadrotor $(arg robot) $(arg x) $(arg y) $(arg z) 0 0 0" 3 | - "$(arg unity_namespace)Spawner spawn MonoCamera cam_left 0.1 0.1 0.0 0 0 0 $(arg robot)" 4 | - "$(arg robot)/cam_left configure width:$(arg cam_width) height:$(arg cam_height)" 5 | - "$(arg unity_namespace)Spawner spawn MonoCamera cam_right 0.1 -0.1 0.0 0 0 0 $(arg robot)" 6 | - "$(arg robot)/cam_right configure width:$(arg cam_width) height:$(arg cam_height)" 7 | - "$(arg unity_namespace)Spawner spawn IMU imu 0 0 0 0 0 0 $(arg robot)" 8 | - "$(arg unity_namespace)Spawner spawn Lidar3D lidar 0.0 0.0 0.05 0 0 0 $(arg robot)" 9 | - "$(arg robot)/lidar configure verticalBeams:32 horizontalBeams:1024" 10 | -------------------------------------------------------------------------------- /autonomy_core/estimation/fla_ukf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(fla_ukf) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | add_definitions(-D VIO_NO_POSITION=0 -D ENABLE_VIO_YAW_OFFSET=0) 6 | 7 | find_package( 8 | catkin REQUIRED 9 | COMPONENTS eigen_conversions 10 | nav_msgs 11 | nodelet 12 | roscpp 13 | sensor_msgs 14 | tf2_ros 15 | angles) 16 | 17 | find_package(Eigen3 REQUIRED) 18 | 19 | catkin_package(INCLUDE_DIRS include) 20 | 21 | add_library(${PROJECT_NAME}_nodelet src/fla_ukf_nodelet.cpp src/fla_ukf.cpp) 22 | target_include_directories(${PROJECT_NAME}_nodelet 23 | PRIVATE include ${catkin_INCLUDE_DIRS}) 24 | target_link_libraries(${PROJECT_NAME}_nodelet PRIVATE ${catkin_LIBRARIES}) 25 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/map_plan_launch/launch/depth_to_cloud.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(coverage_utils) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | kr_planning_msgs 8 | roscpp 9 | ) 10 | 11 | find_package(Eigen3 REQUIRED) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include 15 | CATKIN_DEPENDS 16 | kr_planning_msgs 17 | ) 18 | 19 | include_directories(${PROJECT_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include) 20 | 21 | add_library(${PROJECT_NAME} 22 | src/coverage_utils.cpp 23 | src/utils.cpp 24 | ) 25 | target_link_libraries(${PROJECT_NAME} PUBLIC 26 | ${catkin_LIBRARIES} 27 | Eigen3::Eigen) 28 | 29 | add_executable(coverage_utils_node 30 | src/coverage_utils_node.cpp) 31 | target_link_libraries(coverage_utils_node PUBLIC 32 | ${PROJECT_NAME} ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /autonomy_core/interface/mavros_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mavros_interface 3 | 1.0.0 4 | mavros_interface 5 | Anurag Makineni 6 | Justin Thomas 7 | Kartik Mohta 8 | 9 | Penn Software License 10 | 11 | catkin 12 | 13 | roscpp 14 | nav_msgs 15 | geometry_msgs 16 | kr_mav_msgs 17 | mavros_msgs 18 | nodelet 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /external_all.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | msckf_vio: 3 | type: git 4 | url: http://github.com/KumarRobotics/msckf_vio.git 5 | version: master 6 | 7 | kr_mav_control: 8 | type: git 9 | url: http://github.com/KumarRobotics/kr_mav_control.git 10 | version: master 11 | 12 | mrsl_quadrotor: 13 | type: git 14 | url: http://github.com/KumarRobotics/mrsl_quadrotor.git 15 | version: master 16 | 17 | ouster_example: 18 | type: git 19 | url: http://github.com/XuRobotics/ouster_example.git 20 | version: os-sim 21 | 22 | waypoint_navigation_plugin: 23 | type: git 24 | url: https://github.com/KumarRobotics/waypoint_navigation_plugin.git 25 | version: master 26 | 27 | kr_planning_msgs: 28 | type: git 29 | url: https://github.com/KumarRobotics/kr_planning_msgs.git 30 | version: main 31 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fake_lidar 4 | 0.0.0 5 | The fake_lidar package 6 | Chao Qu 7 | Guilherme Nardari 8 | Xu Liu 9 | 10 | Penn Software License 11 | 12 | 13 | catkin 14 | 15 | roscpp 16 | sensor_msgs 17 | image_transport 18 | image_geometry 19 | cv_bridge 20 | tf2_ros 21 | tf2_eigen 22 | pcl_ros 23 | message_filters 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_core/action/Replan.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | geometry_msgs/Pose p_init 3 | geometry_msgs/Pose[] p_finals # a list of waypoints, if empty, will use single goal (i.e. p_final) 4 | geometry_msgs/Pose p_final # the last waypoint,used to check whether the robot has finished all waypoints when replanning 5 | float32 local_replan_rate 6 | int16 global_replan_rate_factor 7 | bool continue_mission # continue to finish waypoints in existing mission instead of starting a new mission 8 | bool reset 9 | bool avoid_obstacles 10 | --- 11 | # Result 12 | uint8 status 13 | uint8 SUCCESS = 0 14 | uint8 DYNAMICALLY_INFEASIBLE = 1 15 | uint8 NO_PATH = 2 16 | uint8 CRITICAL_ERROR = 3 17 | uint8 IN_PROGRESS = 4 18 | uint8 ABORT_FULL_MISSION = 5 19 | --- 20 | # Feedback 21 | float64 distance_to_goal 22 | int64 epoch 23 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/include/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | // Functions for convex_hull calculation are from 9 | // https://www.topcoder.com/blog/problem-of-the-week-save-the-trees/ 10 | 11 | struct pt { 12 | double x, y; 13 | }; 14 | bool cmp(pt a, pt b); 15 | bool cw(pt a, pt b, pt c); 16 | bool ccw(pt a, pt b, pt c); 17 | template 18 | using Vecf = Eigen::Matrix; 19 | typedef Vecf<2> Vec2f; 20 | typedef Vecf<3> Vec3f; 21 | template 22 | using vec_E = std::vector>; 23 | typedef vec_E vec_Vec2f; 24 | typedef vec_E vec_Vec3f; 25 | kr_planning_msgs::Path path_to_ros(const vec_Vec3f& path, double h = 0); 26 | void convex_hull(std::vector* pts); 27 | std::vector PreprocessData(const std::string& fname); 28 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_launch/launch/smach.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /autonomy_core/client/docker/deploy.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Make sure processes in the container can connect to the x server 4 | # Necessary so gazebo can create a context for OpenGL rendering (even headless) 5 | XAUTH=/tmp/.docker.xauth 6 | if [ ! -f $XAUTH ] 7 | then 8 | xauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/') 9 | if [ ! -z "$xauth_list" ] 10 | then 11 | echo $xauth_list | xauth -f $XAUTH nmerge - 12 | else 13 | touch $XAUTH 14 | fi 15 | chmod a+r $XAUTH 16 | fi 17 | 18 | # TODO: add command line argument for rosmaster uri 19 | docker run --gpus all -it \ 20 | -e DISPLAY \ 21 | -e QT_X11_NO_MITSHM=1 \ 22 | -e XAUTHORITY=$XAUTH \ 23 | -v "$XAUTH:$XAUTH" \ 24 | -v "/tmp/.X11-unix:/tmp/.X11-unix" \ 25 | --rm \ 26 | --privileged \ 27 | --network=host \ 28 | --name client \ 29 | kumarrobotics/autonomy:client \ 30 | roslaunch client_launch client.launch 31 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | additional_commands: 2 | catkin_package: 3 | kwargs: 4 | INCLUDE_DIRS: "*" 5 | LIBRARIES: "*" 6 | CATKIN_DEPENDS: "*" 7 | DEPENDS: "*" 8 | CFG_EXTRAS: "*" 9 | cc_library: 10 | flags: 11 | - INTERFACE 12 | kwargs: 13 | HDRS: "*" 14 | SRCS: "*" 15 | COPTS: "*" 16 | DEFINES: "*" 17 | LINKOPTS: "*" 18 | DEPS: "*" 19 | INCS: "*" 20 | cc_binary: 21 | kwargs: 22 | SRCS: "*" 23 | COPTS: "*" 24 | DEFINES: "*" 25 | LINKOPTS: "*" 26 | DEPS: "*" 27 | cc_test: 28 | kwargs: 29 | SRCS: "*" 30 | COPTS: "*" 31 | DEFINES: "*" 32 | LINKOPTS: "*" 33 | DEPS: "*" 34 | cc_bench: 35 | kwargs: 36 | SRCS: "*" 37 | COPTS: "*" 38 | DEFINES: "*" 39 | LINKOPTS: "*" 40 | DEPS: "*" 41 | dangle_parens: false 42 | line_ending: unix 43 | line_width: 80 44 | tab_size: 2 45 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mapper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mapper 4 | 0.0.1 5 | The mapper package 6 | sikang 7 | Xu Liu 8 | Guilherme Nardari 9 | Chao Qu 10 | 11 | Penn Software License 12 | 13 | catkin 14 | 15 | roscpp 16 | pcl_ros 17 | image_geometry 18 | kr_planning_msgs 19 | kr_planning_rviz_plugins 20 | sensor_msgs 21 | depth_image_proc 22 | tf_conversions 23 | eigen_conversions 24 | 25 | 26 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_core/include/state_machine/traj_opt_utils.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace state_machine { 9 | 10 | // for new traj_opt backend, can only have 3d trajectories 11 | traj_opt::VecD Make4d(const traj_opt::VecD& vec); 12 | 13 | void VecToPose(const traj_opt::VecD& valn, geometry_msgs::Pose* pos); 14 | void VecToTwist(const traj_opt::VecD& valn, geometry_msgs::Twist* vel); 15 | 16 | // evaluate trajectory into pos, vel, acc, jrk 17 | void EvaluateToMsgs(boost::shared_ptr traj, 18 | double dt, 19 | geometry_msgs::Pose* pos, 20 | geometry_msgs::Twist* vel, 21 | geometry_msgs::Twist* acc, 22 | geometry_msgs::Twist* jrk); 23 | 24 | } // namespace state_machine 25 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | ##### planning server##### 8 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/so3_params_explained.txt: -------------------------------------------------------------------------------- 1 | gains: 2 | pos: {x: 7.4, y: 7.4, z: 10.4} # position gains 3 | vel: {x: 4.8, y: 4.8, z: 6.0} # velocity gains 4 | ki: {x: 0.00, y: 0.00, z: 0.00} # position error integral gains 5 | kib: {x: 0.00, y: 0.00, z: 0.00} # body-frame position error integral gains, NOT used in actual control calculation, not influential 6 | rot: {x: 1.5, y: 1.5, z: 1.0} # rotation gains, only used in so3_trpy_control, NOT used in so3_control, not influential 7 | ang: {x: 0.13, y: 0.13, z: 0.1} # angular velocity gains, only used in so3_trpy_control, NOT used in so3_control, not influential 8 | 9 | corrections: # corrections are only used in so3_trpy_control, NOT used in so3_control, not influential 10 | kf: 0.0e-08 11 | r: 0.0 12 | p: 0.0 13 | 14 | max_pos_int: 0.5 # position error integral term limit, if ki=0, this term is not influential 15 | max_pos_int_b: 0.5 # body-frame position error integral term limit, NOT used in actual control calculation, not influential 16 | -------------------------------------------------------------------------------- /autonomy_core/client/rqt_quadrotor_safety/package.xml: -------------------------------------------------------------------------------- 1 | 2 | rqt_quadrotor_safety 3 | 0.1.0 4 | Provides a GUI plugin for MAV Manager. 5 | Dinesh Thakur 6 | 7 | Penn Software License 8 | 9 | http://ros.org/wiki/rqt_quadrotor_safety 10 | https://github.com/KumarRobotics/kr_ui 11 | 12 | Dinesh Thakur 13 | 14 | catkin 15 | 16 | geometry_msgs 17 | kr_planning_msgs 18 | mavros_msgs 19 | python-rospkg 20 | rostopic 21 | rqt_gui 22 | rqt_gui_py 23 | 24 | mav_manager 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/include/action_trackers/traj_to_quad_cmd.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "traj_opt_ros/trajectory.h" // Trajectory 7 | 8 | namespace traj_opt { 9 | 10 | kr_mav_msgs::PositionCommand EvaluateTrajectory( 11 | const boost::shared_ptr &traj, double dt, 12 | uint max_derr_eval = 3, double scaling = 1.0); 13 | 14 | void EvaluateTrajectory(const boost::shared_ptr &traj, double dt, 15 | kr_mav_msgs::PositionCommand *out, 16 | uint max_derr_eval = 3, double scaling = 1.0); 17 | 18 | // LQR based trajectory tracking 19 | bool EvaluateTrajectoryPos(const boost::shared_ptr &traj, 20 | const nav_msgs::Odometry::ConstPtr &odom, 21 | double err_max, double t_des, double ddt, 22 | kr_mav_msgs::PositionCommand *out); 23 | 24 | } // namespace traj_opt 25 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/odom2tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/msg_traj.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Michael Watterson 2 | #pragma once 3 | #include 4 | 5 | #include "traj_opt_ros/polynomial_basis.h" 6 | #include "traj_opt_ros/traj_data.h" 7 | #include "traj_opt_ros/trajectory.h" 8 | 9 | namespace traj_opt { 10 | 11 | class MsgTrajectory : public Trajectory { 12 | public: 13 | explicit MsgTrajectory(const TrajData &traj); 14 | 15 | bool evaluate(double t, uint derr, VecD &out) const override; 16 | // bool evaluate(double t, VecD &out); 17 | bool evaluateS(double t, VecD &out); 18 | bool evaluateST(double t, VecD &out); 19 | double getTotalTime() const override; 20 | double getCost() override; 21 | TrajData serialize() override; 22 | 23 | protected: 24 | TrajData traj_; 25 | std::vector>> polyies_; 26 | std::vector>>> derrives_; 27 | uint num_secs_; 28 | std::vector dts; 29 | uint deg_; 30 | // int seg; 31 | }; 32 | 33 | } // namespace traj_opt 34 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(dcist_utils) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp nav_msgs tf2_ros) 7 | find_package(Eigen3 REQUIRED NO_MODULE) 8 | find_package(yaml-cpp REQUIRED) 9 | 10 | catkin_package() 11 | 12 | add_executable(odom2tf src/odom2tf.cpp) 13 | target_include_directories(odom2tf PRIVATE ${catkin_INCLUDE_DIRS}) 14 | target_link_libraries(odom2tf PRIVATE ${catkin_LIBRARIES} Eigen3::Eigen) 15 | 16 | add_executable(msckf_calib_gen src/msckf_calib_gen.cpp) 17 | target_include_directories(msckf_calib_gen PRIVATE ${YAML_CPP_INCLUDE_DIR} 18 | ${catkin_INCLUDE_DIRS}) 19 | target_link_libraries( 20 | msckf_calib_gen PRIVATE ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES} 21 | Eigen3::Eigen) 22 | 23 | add_executable(playground src/playground.cpp) 24 | target_include_directories(playground PRIVATE ${catkin_INCLUDE_DIRS}) 25 | target_link_libraries(playground PRIVATE ${catkin_LIBRARIES}) 26 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/launch/depth2cloud.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/types.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Michael Watterson 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace traj_opt { 9 | 10 | typedef Eigen::Matrix Vec3; 11 | typedef Eigen::Matrix Vec4; 12 | typedef Eigen::Matrix Vec5; 13 | typedef Eigen::Matrix Vec6; 14 | typedef Eigen::Matrix Mat3; 15 | typedef Eigen::Matrix Mat4; 16 | 17 | typedef std::vector> Vec4Vec; 18 | typedef std::vector> Vec3Vec; 19 | typedef std::vector> Mat4Vec; 20 | 21 | typedef Eigen::Matrix VecD; 22 | typedef Eigen::Matrix MatD3; 23 | typedef Eigen::Matrix MatD; 24 | 25 | typedef std::vector VecDVec; 26 | typedef std::vector MatDVec; 27 | 28 | typedef Eigen::Quaternion Quat; 29 | 30 | } // namespace traj_opt 31 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/src/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace fake_lidar { 9 | using PointPCL = pcl::PointXYZI; 10 | using PointCV = cv::Point3d; 11 | 12 | template 13 | void EuclideanSpherical(double radius, double alt, double azimuth, PointT& pt) { 14 | pt.x = -radius * cos(alt) * sin(azimuth); 15 | pt.y = -radius * sin(alt); 16 | pt.z = radius * cos(alt) * cos(azimuth); 17 | } 18 | 19 | 20 | static constexpr double PointAltitude(double x, double y, double z) { 21 | // [-pi, pi] 22 | return std::atan2(z, std::hypot(x, y)); 23 | } 24 | 25 | static constexpr double PointAzimuth(double x, double y) { 26 | const auto a = std::atan2(y, x); 27 | // Convert to [0, 2pi) 28 | return y >= 0 ? a : a + M_PI * 2; 29 | } 30 | /// Degree from Radian 31 | static constexpr double Deg_Rad(double rad) { return rad * 180 / M_PI; } 32 | 33 | /// Radian from Degree 34 | static constexpr double Rad_Deg(double deg) { return deg / 180 * M_PI; } 35 | } // namespace fake_lidar -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/tracker_params.yaml: -------------------------------------------------------------------------------- 1 | # This should contain tracker parameters 2 | 3 | #line_tracker_distance: 4 | # default_v_des: 2.5 5 | # default_a_des: 1.5 6 | # default_w_des: 2.5 7 | # epsilon: 0.1 8 | 9 | #line_tracker_min_jerk: 10 | # default_v_des: 2.5 11 | # default_a_des: 1.5 12 | # default_yaw_v_des: 1.0 13 | # default_yaw_a_des: 0.5 14 | 15 | stopping_policy: 16 | default_a_des: 1.0 17 | default_j_des: 2.0 18 | 19 | take_off_tracker: 20 | thrust_rate: 5.0 21 | max_thrust: 5.0 22 | min_thurst: -9.8 23 | epsilon: 0.01 24 | 25 | path_tracker: 26 | v_max: 2.5 27 | a_max: 0.2 28 | v_min: 0.3 29 | r_max: 0.8 30 | r_min: 0.1 31 | outer_r_max: 4.0 32 | robot_r: 0.2 # robot radius 33 | theta_thr: 1.57 34 | yaw_thr: 0.3 35 | yaw_speed: 0.35 36 | 37 | land_tracker: 38 | vel: 0.5 39 | acc: 0.5 40 | epsilon: 0.5 41 | 42 | #trajectory_tracker: 43 | # max_vel_des: 4.0 44 | # max_acc_des: 5.0 45 | 46 | #velocity_tracker: 47 | # timeout: 0.5 48 | 49 | #lissajous_tracker: 50 | # frame_id: simulator 51 | 52 | #lissajous_adder: 53 | # frame_id: simulator 54 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(fake_lidar) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | list(PREPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") 7 | 8 | find_package( 9 | catkin REQUIRED 10 | COMPONENTS roscpp 11 | sensor_msgs 12 | image_geometry 13 | image_transport 14 | message_filters 15 | cv_bridge 16 | tf2_eigen 17 | pcl_ros 18 | tf2_ros) 19 | 20 | find_package(Boost REQUIRED COMPONENTS timer) 21 | find_package(TBB REQUIRED) 22 | 23 | catkin_package() 24 | 25 | add_executable(fake_lidar_node src/fake_lidar_node.cpp) 26 | target_include_directories(fake_lidar_node PRIVATE ${catkin_INCLUDE_DIRS}) 27 | target_link_libraries(fake_lidar_node PRIVATE ${catkin_LIBRARIES} Boost::timer) 28 | 29 | add_executable(fake_lidar_node2 src/fake_lidar_node2.cpp) 30 | target_include_directories(fake_lidar_node2 PRIVATE ${catkin_INCLUDE_DIRS}) 31 | target_link_libraries(fake_lidar_node2 PRIVATE ${catkin_LIBRARIES} Boost::timer 32 | TBB::tbb) 33 | -------------------------------------------------------------------------------- /.github/workflows/cpplint-reviewdog.yaml: -------------------------------------------------------------------------------- 1 | name: cpplint-reviewdog 2 | on: [pull_request] 3 | 4 | jobs: 5 | cpplint: 6 | runs-on: ubuntu-latest 7 | steps: 8 | - uses: actions/checkout@master 9 | - uses: reviewdog/action-cpplint@master 10 | with: 11 | github_token: ${{ secrets.KR_AUTONOMOUS_FLIGHT_TOKEN_REVIEWDOG }} 12 | reporter: github-pr-check 13 | flags: --exclude=autonomy_core/map_plan/jps3d/include/jps/graph_search.h --exclude autonomy_core/map_plan/jps3d/src/graph_search.cpp --exclude autonomy_core/state_machine/action_trackers/src/take_off_tracker.cpp --exclude autonomy_core/map_plan/mpl/include/mpl_basis/lambda.h --exclude autonomy_core/map_plan/mpl/include/mpl_planner/env_base.h --exclude autonomy_core/map_plan/mpl/src/env_base.cpp --exclude autonomy_core/map_plan/mpl/src/env_map.cpp 14 | filter: "-whitespace/comments,-whitespace/indent,-build/include_order,-whitespace/ending_newline,-runtime/references" 15 | # Note: runtime/references was added to address google benchmark 16 | # issues, as requested by https://github.com/KumarRobotics/kr_autonomous_flight/pull/171 17 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/msckf_calib.yaml: -------------------------------------------------------------------------------- 1 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2 | 0.0, 1.0] 3 | cam0: 4 | T_cam_imu: [-0.99999007961, -0.00132207465, 0.00425356229, 0.09139940757, -0.001326278, 5 | 0.99999863486, -0.00098552559, 0.01560966073, -0.00425225354, -0.00099115722, 6 | -0.99999046793, -0.0172675311, 0.0, 0.0, 0.0, 1.0] 7 | camera_model: pinhole 8 | distortion_coeffs: [-0.0921668, -0.00378887, 0.0171497, -0.0200551] 9 | distortion_model: equidistant 10 | intrinsics: [1054.29, 1054.67, 653.695, 329.993] 11 | resolution: [1280, 800] 12 | rostopic: /ovc/left/image_raw 13 | cam1: 14 | T_cn_cnm1: [0.99999919518, 0.00109592589, 0.00063920742, -0.11973430965, -0.00109347033, 15 | 0.99999207021, -0.00382934007, -5.872744e-05, -0.00064339903, 0.00382863803, 0.99999246376, 16 | -0.00057991569, 0.0, 0.0, 0.0, 1.0] 17 | camera_model: pinhole 18 | distortion_coeffs: [-0.104196, 0.0630347, -0.121242, 0.0796202] 19 | distortion_model: equidistant 20 | intrinsics: [1057.41, 1058.18, 685.506, 336.12] 21 | resolution: [1280, 800] 22 | rostopic: /ovc/right/image_raw 23 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/trajectory.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Michael Watterson 2 | #pragma once 3 | 4 | #include // pair 5 | 6 | #include "traj_opt_ros/traj_data.h" 7 | #include "traj_opt_ros/types.h" 8 | 9 | namespace traj_opt { 10 | 11 | class Trajectory { 12 | public: 13 | virtual ~Trajectory() {} 14 | virtual bool evaluate(double t, uint derr, VecD &out) const = 0; 15 | virtual double getTotalTime() const = 0; 16 | virtual double getCost() = 0; 17 | // execute time 18 | double getExecuteTime() const; 19 | std::pair getXi(double t, double g = 9.81); 20 | std::pair getYaws(double t); 21 | bool getHopfChart(double t); 22 | 23 | // returns a matrix (dim X num_derivatives + 1) of the trajectory evalutated 24 | // at time t 25 | bool getCommand(double t, uint num_derivatives, MatD &data); 26 | void setDim(uint ndim) { dim_ = ndim; } 27 | void setExecuteTime(double t) { exec_t = t; } 28 | 29 | int getDim() { return dim_; } 30 | virtual TrajData serialize() = 0; 31 | 32 | protected: 33 | double exec_t{-1.0}; // duration of execute time 34 | int dim_{0}; 35 | }; 36 | 37 | } // namespace traj_opt 38 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/throttle_map_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | state_machine 4 | 0.0.0 5 | The state_machine package 6 | 7 | Mike Watterson 8 | Xu Liu 9 | Yuezhan Tao 10 | Chao Qu 11 | 12 | Penn Software License 13 | 14 | 15 | catkin 16 | action_planner 17 | action_trackers 18 | actionlib 19 | tf2_ros 20 | actionlib_msgs 21 | kr_mav_msgs 22 | sensor_msgs 23 | tf_conversions 24 | eigen_conversions 25 | 26 | traj_opt_ros 27 | kr_planning_rviz_plugins 28 | kr_planning_msgs 29 | geometry_msgs 30 | smach_ros 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /autonomy_core/interface/scripts/install_geographiclib_datasets.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to install the model datasets required 3 | # to GeographicLib apply certain conversions 4 | 5 | if [[ $UID != 0 ]]; then 6 | echo "This script require root privileges!" 1>&2 7 | exit 1 8 | fi 9 | 10 | # Install datasets 11 | run_get() { 12 | local dir="$1" 13 | local tool="$2" 14 | local model="$3" 15 | 16 | files=$(shopt -s nullglob dotglob; echo /usr/share/GeographicLib/$dir/$model* /usr/local/share/GeographicLib/$dir/$model*) 17 | if (( ${#files} )); then 18 | echo "GeographicLib $tool dataset $model already exists, skipping" 19 | return 20 | fi 21 | 22 | echo "Installing GeographicLib $tool $model" 23 | geographiclib-get-$tool $model >/dev/null 2>&1 24 | } 25 | 26 | # check which command script is available 27 | if hash geographiclib-get-geoids; then 28 | run_get geoids geoids egm96-5 29 | run_get gravity gravity egm96 30 | run_get magnetic magnetic emm2015 31 | elif hash geographiclib-datasets-download; then # only allows install the goid model dataset 32 | geographiclib-datasets-download egm96_5; 33 | else 34 | echo "OS not supported! Check GeographicLib page for supported OS and lib versions." 1>&2 35 | fi -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/kalibr-8-30-2021/result-1st-success/initial-camera.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_overlaps: [1] 3 | camera_model: pinhole 4 | distortion_coeffs: [-0.08921669100012515, 0.0036755424278144865, -0.011708374532048397, 5 | 0.005799357286396118] 6 | distortion_model: equidistant 7 | intrinsics: [1058.04703898802, 1058.2196100463393, 642.0867566020861, 343.6212525674095] 8 | resolution: [1280, 800] 9 | rostopic: /ovc/left/image_raw 10 | cam1: 11 | T_cn_cnm1: 12 | - [0.999996654231944, 0.00022298796413894854, -0.0025771692391961965, -0.11997563643258276] 13 | - [-0.00021389657269771454, 0.9999937558183895, 0.0035274030800726635, 6.0396888206409554e-05] 14 | - [0.0025779397153149466, -0.003526840030532588, 0.9999904577675853, -0.0012829974512676998] 15 | - [0.0, 0.0, 0.0, 1.0] 16 | cam_overlaps: [0] 17 | camera_model: pinhole 18 | distortion_coeffs: [-0.09337087156090869, 0.026346618183340113, -0.06329486672350314, 19 | 0.04644342995151147] 20 | distortion_model: equidistant 21 | intrinsics: [1055.434094454324, 1055.6845447978794, 666.9265866693742, 351.85682800429487] 22 | resolution: [1280, 800] 23 | rostopic: /ovc/right/image_raw 24 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mapper/src/tf_listener.cpp: -------------------------------------------------------------------------------- 1 | #include "mapper/tf_listener.h" 2 | 3 | namespace mapper { 4 | 5 | std::optional TFListener::LookupTransform( 6 | const std::string &target, const std::string &source, 7 | const ros::Time &time) { 8 | geometry_msgs::TransformStamped transformStamped; 9 | try { 10 | transformStamped = 11 | buffer_.lookupTransform(target, source, time, ros::Duration(0.4)); 12 | } catch (tf2::TransformException &ex) { 13 | ROS_WARN_THROTTLE(1, "Fail to find transform from [%s] to [%s]", 14 | source.c_str(), target.c_str()); 15 | return {}; 16 | } 17 | 18 | geometry_msgs::Pose pose; 19 | 20 | pose.position.x = transformStamped.transform.translation.x; 21 | pose.position.y = transformStamped.transform.translation.y; 22 | pose.position.z = transformStamped.transform.translation.z; 23 | pose.orientation.w = transformStamped.transform.rotation.w; 24 | pose.orientation.x = transformStamped.transform.rotation.x; 25 | pose.orientation.y = transformStamped.transform.rotation.y; 26 | pose.orientation.z = transformStamped.transform.rotation.z; 27 | 28 | return pose; 29 | } 30 | 31 | } // namespace mapper 32 | -------------------------------------------------------------------------------- /.github/workflows/docker-build-base.yaml: -------------------------------------------------------------------------------- 1 | name: docker-build-base 2 | 3 | # Only build base when any of the files in the base directory are modified 4 | on: 5 | push: 6 | paths: 7 | - 'autonomy_core/base/**' 8 | branches: [master] 9 | workflow_dispatch: 10 | schedule: 11 | - cron: '0 0 * * 0' 12 | 13 | jobs: 14 | main: 15 | runs-on: ubuntu-20.04 16 | 17 | steps: 18 | - 19 | name: Set up QEMU 20 | uses: docker/setup-qemu-action@v1 21 | - 22 | name: Set up Docker Buildx 23 | uses: docker/setup-buildx-action@v1 24 | - 25 | name: Login to DockerHub 26 | uses: docker/login-action@v1 27 | with: 28 | username: ${{ secrets.DOCKERHUB_USERNAME }} 29 | password: ${{ secrets.DOCKERHUB_TOKEN }} 30 | - 31 | name: Build and push 32 | id: docker_build 33 | uses: docker/build-push-action@v2 34 | with: 35 | push: true 36 | tags: kumarrobotics/autonomy:base 37 | file: ./autonomy_core/base/Dockerfile 38 | platforms: linux/amd64,linux/arm64 39 | - 40 | name: Image digest 41 | run: echo ${{ steps.docker_build.outputs.digest }} 42 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/old-launch/throttle_odom_img.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/src/data_conversions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | void setMap(const std::shared_ptr& map_util, 10 | const kr_planning_msgs::VoxelMap& msg); 11 | 12 | void getMap(const std::shared_ptr& map_util, 13 | kr_planning_msgs::VoxelMap* map); 14 | 15 | void setMap(const std::shared_ptr& map_util, 16 | const kr_planning_msgs::VoxelMap& msg); 17 | 18 | void getMap(const std::shared_ptr& map_util, 19 | kr_planning_msgs::VoxelMap* map); 20 | 21 | void setMap(const std::shared_ptr& map_util, 22 | const kr_planning_msgs::VoxelMap& msg); 23 | 24 | void getMap(const std::shared_ptr& map_util, 25 | kr_planning_msgs::VoxelMap* map); 26 | 27 | // NOTE: This function is the same as getInflatedOccMap function in 28 | // voxel_mapper.cpp, should merge them. 29 | kr_planning_msgs::VoxelMap sliceMap(const kr_planning_msgs::VoxelMap& map, 30 | double h, 31 | double hh = 0); 32 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/action_trackers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | action_trackers 4 | 0.1.1 5 | The action_trackers package 6 | 7 | Mike Watterson 8 | Kartik Mohta 9 | Sikang Liu 10 | Xu Liu 11 | Chao Qu 12 | 13 | Penn Software License 14 | 15 | catkin 16 | 17 | roscpp 18 | actionlib 19 | actionlib_msgs 20 | 21 | kr_trackers 22 | kr_mav_msgs 23 | kr_tracker_msgs 24 | kr_trackers_manager 25 | 26 | mapper 27 | kr_planning_rviz_plugins 28 | kr_planning_msgs 29 | traj_opt_ros 30 | motion_primitive_library 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | action_planner 4 | 0.0.0 5 | The action_planner package 6 | 7 | 8 | Xu Liu 9 | Yuezhan Tao 10 | Chao Qu 11 | 12 | Penn Software License 13 | 14 | catkin 15 | message_generation 16 | message_runtime 17 | actionlib_msgs 18 | actionlib_msgs 19 | 20 | dynamic_reconfigure 21 | roscpp 22 | actionlib 23 | eigen_conversions 24 | tf2_ros 25 | tf_conversions 26 | pcl_ros 27 | mapper 28 | jps3d 29 | kr_planning_rviz_plugins 30 | kr_planning_msgs 31 | traj_opt_ros 32 | motion_primitive_library 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /autonomy_core/interface/mavros_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(mavros_interface) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(catkin REQUIRED COMPONENTS roscpp nav_msgs geometry_msgs 6 | mavros_msgs kr_mav_msgs nodelet) 7 | find_package(Eigen3 REQUIRED) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS 11 | LIBRARIES 12 | CATKIN_DEPENDS 13 | roscpp 14 | nav_msgs 15 | geometry_msgs 16 | kr_mav_msgs 17 | mavros_msgs 18 | nodelet) 19 | 20 | add_library(so3cmd_to_mavros_nodelet src/so3cmd_to_mavros_nodelet.cpp) 21 | target_include_directories(so3cmd_to_mavros_nodelet 22 | PUBLIC ${catkin_INCLUDE_DIRS}) 23 | target_link_libraries(so3cmd_to_mavros_nodelet PUBLIC ${catkin_LIBRARIES} 24 | Eigen3::Eigen) 25 | 26 | install( 27 | TARGETS so3cmd_to_mavros_nodelet 28 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 29 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 30 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 31 | 32 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 33 | install(FILES nodelet_plugins.xml 34 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 35 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/msckf_calib-8-11.yaml: -------------------------------------------------------------------------------- 1 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2 | 0.0, 1.0] 3 | cam0: 4 | T_cam_imu: [-0.99991370721, 0.00669023713, 0.01130570059, 0.09153893768, 5 | 0.00673396934, 0.99996997458, 0.00383452572, 0.01258894695, 6 | -0.01127970725, 0.00391032707, -0.99992873623, 0.01285669899, 7 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 8 | camera_model: pinhole 9 | intrinsics: [1063.44, 1063.24, 641.234, 343.333] 10 | distortion_model: equidistant 11 | distortion_coeffs: [-0.0919058, 0.00522154, -0.0105818, 0.00306086] 12 | resolution: [1280, 800] 13 | rostopic: /ovc/left/image_raw 14 | cam1: 15 | T_cn_cnm1: [ 0.99999188474, 0.00015326950, -0.00402578753, -0.11982485729, 16 | -0.00012053026, 0.99996693264, 0.00813136458, 0.00014283334, 17 | 0.00402690069, -0.00813081336, 0.99995883613, -0.00098466553, 18 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 19 | camera_model: pinhole 20 | intrinsics: [1059.84, 1059.68, 663.88, 350.121] 21 | distortion_model: equidistant 22 | distortion_coeffs: [-0.0905249, -0.0110959, 0.0210461, -0.0157691] 23 | resolution: [1280, 800] 24 | rostopic: /ovc/right/image_raw 25 | -------------------------------------------------------------------------------- /autonomy_core/client/client_launch/launch/client.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/msckf_calib-8-23-final.yaml: -------------------------------------------------------------------------------- 1 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2 | 0.0, 1.0] 3 | cam0: 4 | T_cam_imu: [-0.99991370721, 0.00669023713, 0.01130570059, 0.09153893768, 5 | 0.00673396934, 0.99996997458, 0.00383452572, 0.01258894695, 6 | -0.01127970725, 0.00391032707, -0.99992873623, 0.01285669899, 7 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 8 | camera_model: pinhole 9 | intrinsics: [1060.45, 1060.69, 640.877, 344.788] 10 | distortion_model: equidistant 11 | distortion_coeffs: [-0.0924296, 0.0135224, -0.0300342, 0.0195595] 12 | resolution: [1280, 800] 13 | rostopic: /ovc/left/image_raw 14 | cam1: 15 | T_cn_cnm1: [0.99998479950, 0.00007664300, -0.00551315692, -0.11996458358, 16 | -0.00005273294, 0.99999059411, 0.00433692470, 0.00005949042, 17 | 0.00551343746, -0.00433656806, 0.99997539779, -0.00091282711, 18 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 19 | camera_model: pinhole 20 | intrinsics: [1057.11, 1057.47, 663.208, 350.336] 21 | distortion_model: equidistant 22 | distortion_coeffs: [-0.0897924, -0.00547218, 0.00196313, 0.00315461] 23 | resolution: [1280, 800] 24 | rostopic: /ovc/right/image_raw 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/map_plan_launch/launch/mapper.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 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/ros_bridge.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Michael Watterson 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "traj_opt_ros/traj_data.h" 11 | 12 | namespace traj_opt { 13 | 14 | class TrajRosBridge { 15 | public: 16 | // make sure to run ros::init() before calling this function or it won't work 17 | static void publish_msg(const kr_planning_msgs::SplineTrajectory &msg, 18 | std::string frame_id = "map"); 19 | 20 | static void publish_msg(const TrajData &data, std::string frame_id = "map"); 21 | 22 | // Use global singleton paradignm. All these things are private! 23 | // Keep your government out of my contructors! 24 | private: 25 | TrajRosBridge(); 26 | static TrajRosBridge &instance(); 27 | ros::NodeHandle nh_; 28 | ros::Publisher pub_; 29 | }; 30 | 31 | kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajData( 32 | const TrajData &data); 33 | 34 | TrajData TrajDataFromSplineTrajectory( 35 | const kr_planning_msgs::SplineTrajectory &msg); 36 | 37 | kr_planning_msgs::SplineTrajectory SplineTrajectoryFromTrajectory( 38 | const kr_planning_msgs::Trajectory &msg); 39 | 40 | } // namespace traj_opt 41 | -------------------------------------------------------------------------------- /autonomy_core/estimation/estimation_launch/config/msckf_calib_template.yaml: -------------------------------------------------------------------------------- 1 | # The modifications of the output file from Kalibr: 2 | # 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix. 3 | # 2. Add the T_imu_body at the end of the calibration file (usually set to identity). 4 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 5 | 0.0, 1.0] 6 | cam0: 7 | T_cam_imu: 8 | [0.0,-1.0, 0.0, 0.1, 9 | 0.0, 0.0,-1.0, 0.0, 10 | 1.0, 0.0, 0.0, -0.1, 11 | 0.0, 0.0, 0.0, 1.0] 12 | camera_model: pinhole 13 | intrinsics: [130.499, 130.499, 160.0, 120.0] 14 | distortion_model: radtan 15 | distortion_coeffs: [0.0, 0.0, 0.0, 0.0] 16 | resolution: [480, 320] 17 | rostopic: /quadrotor/camera_left/image_raw 18 | cam1: 19 | T_cam_imu: 20 | [0.0,-1.0, 0.0, -0.1, 21 | 0.0, 0.0,-1.0, 0.0, 22 | 1.0, 0.0, 0.0, -0.1, 23 | 0.0, 0.0, 0.0, 1.0] 24 | T_cn_cnm1: 25 | [1.0, 0.0, 0.0, -0.2, 26 | 0.0, 1.0, 0.0, 0.0, 27 | 0.0, 0.0, 1.0, 0.0, 28 | 0.0, 0.0, 0.0, 1.0] 29 | camera_model: pinhole 30 | intrinsics: [130.499, 130.499, 160.0, 120.0] 31 | distortion_model: radtan 32 | distortion_coeffs: [0.0, 0.0, 0.0, 0.0] 33 | resolution: [480, 320] 34 | rostopic: /quadrotor/camera_right/image_raw 35 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/record_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 43 | 44 | -------------------------------------------------------------------------------- /autonomy_core/estimation/estimation_launch/launch/msckf_imgproc.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 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/map_plan_launch/launch/mp_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | ##### planning server##### 10 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/include/mpl_basis/lambda.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file lambda.h 3 | * @brief Lambda class 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | 10 | #include "mpl_basis/data_type.h" 11 | namespace MPL { 12 | 13 | /** 14 | * @brief Used for scaling, ignored for most case 15 | */ 16 | struct VirtualPoint { 17 | decimal_t p; 18 | decimal_t v; 19 | decimal_t t; 20 | }; 21 | 22 | /** 23 | * @brief polynomial between two virtual points 24 | */ 25 | class LambdaSeg { 26 | public: 27 | LambdaSeg() = default; 28 | LambdaSeg(const VirtualPoint& v1, const VirtualPoint& v2); 29 | 30 | VirtualPoint evaluate(decimal_t tau) const; 31 | decimal_t getT(decimal_t t) const; 32 | 33 | Vec4f a; ///< a3, a2, a1, a0 34 | decimal_t ti; 35 | decimal_t tf; 36 | decimal_t dT; 37 | }; 38 | 39 | /** 40 | * @brief piecewise polynomial for scaling trajectory 41 | * 42 | */ 43 | class Lambda { 44 | public: 45 | Lambda() = default; 46 | Lambda(const std::vector& vs); 47 | 48 | bool exist() const { return !segs.empty(); } 49 | std::vector sample(int N); 50 | vec_Vec3f sampleT(int N); 51 | VirtualPoint evaluate(decimal_t tau) const; 52 | decimal_t getT(decimal_t tau) const; 53 | decimal_t getTau(decimal_t t) const; 54 | decimal_t getTotalTime() const; 55 | 56 | std::vector segs; 57 | }; 58 | 59 | } // namespace MPL 60 | -------------------------------------------------------------------------------- /.github/workflows/docker-build-estimation.yaml: -------------------------------------------------------------------------------- 1 | name: docker-build-estimation 2 | 3 | # Build estimation when "base" changes, or when any files in the estimation 4 | # folder are modified 5 | on: 6 | workflow_run: 7 | workflows: ["docker-build-base"] 8 | branches: [master] 9 | types: 10 | - completed 11 | push: 12 | paths: 13 | - 'autonomy_core/estimation/**' 14 | branches: [master] 15 | workflow_dispatch: 16 | # DO NOT add a schedule. This build will be triggered by base schedule. 17 | 18 | jobs: 19 | main: 20 | runs-on: ubuntu-20.04 21 | 22 | steps: 23 | - 24 | name: Set up QEMU 25 | uses: docker/setup-qemu-action@v1 26 | - 27 | name: Set up Docker Buildx 28 | uses: docker/setup-buildx-action@v1 29 | - 30 | name: Login to DockerHub 31 | uses: docker/login-action@v1 32 | with: 33 | username: ${{ secrets.DOCKERHUB_USERNAME }} 34 | password: ${{ secrets.DOCKERHUB_TOKEN }} 35 | - 36 | name: Build and push 37 | id: docker_build 38 | uses: docker/build-push-action@v2 39 | with: 40 | push: true 41 | tags: kumarrobotics/autonomy:estimation 42 | file: ./autonomy_core/estimation/Dockerfile 43 | platforms: linux/amd64,linux/arm64 44 | - 45 | name: Image digest 46 | run: echo ${{ steps.docker_build.outputs.digest }} 47 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_launch/launch/state_machine_mp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | ##### motion primitive replanner##### 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/script/test_hash.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/mpl_basis/control.h" 2 | #include "../include/mpl_basis/waypoint.h" 3 | #include "boost/unordered_map.hpp" 4 | 5 | template 6 | std::size_t hash_value(const Waypoint &key) { 7 | // typedef Waypoint argument_type; 8 | // typedef std::size_t result_type; 9 | std::size_t val = 0; 10 | for (int i = 0; i < Dim; i++) { 11 | if (key.use_pos) { 12 | int id = std::round(key.pos(i) / 0.1); 13 | boost::hash_combine(val, id); 14 | } 15 | if (key.use_vel) { 16 | int id = std::round(key.vel(i) / 0.1); 17 | boost::hash_combine(val, id); 18 | } 19 | if (key.use_acc) { 20 | int id = std::round(key.acc(i) / 0.1); 21 | boost::hash_combine(val, id); 22 | } 23 | } 24 | 25 | return val; 26 | } 27 | 28 | boost::unordered_map> votes; 29 | 30 | int main() { 31 | Waypoint2D w1; 32 | w1.pos = Vec2f(0, 0); 33 | w1.vel = Vec2f(0, 0); 34 | w1.acc = Vec2f(0, 0); 35 | w1.use_pos = true; 36 | w1.use_vel = true; 37 | w1.use_acc = true; 38 | 39 | Waypoint2D w2; 40 | w2.pos = Vec2f(0, 0); 41 | w2.vel = Vec2f(0, 0.1); 42 | w2.acc = Vec2f(0, 0); 43 | w2.use_pos = true; 44 | w2.use_vel = true; 45 | w2.use_acc = true; 46 | 47 | std::cout << "w1: " << hash_value(w1) << std::endl; 48 | std::cout << "w2: " << hash_value(w2) << std::endl; 49 | 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/publish_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/src/waypoint.cpp: -------------------------------------------------------------------------------- 1 | #include "mpl_basis/waypoint.h" 2 | 3 | #include 4 | 5 | namespace MPL { 6 | 7 | template 8 | void Waypoint::print(std::string str) const { 9 | if (!str.empty()) std::cout << str << std::endl; 10 | if (use_pos) std::cout << "pos: " << pos.transpose() << std::endl; 11 | if (use_vel) std::cout << "vel: " << vel.transpose() << std::endl; 12 | if (use_acc) std::cout << "acc: " << acc.transpose() << std::endl; 13 | if (use_jrk) std::cout << "jrk: " << jrk.transpose() << std::endl; 14 | if (use_yaw) std::cout << "yaw: " << yaw << std::endl; 15 | if (enable_t) std::cout << " t: " << t << std::endl; 16 | 17 | if (control == MPL::VEL) 18 | std::cout << "use vel!" << std::endl; 19 | else if (control == MPL::ACC) 20 | std::cout << "use acc!" << std::endl; 21 | else if (control == MPL::JRK) 22 | std::cout << "use jrk!" << std::endl; 23 | else if (control == MPL::SNP) 24 | std::cout << "use snp!" << std::endl; 25 | else if (control == MPL::VELxYAW) 26 | std::cout << "use vel & yaw!" << std::endl; 27 | else if (control == MPL::ACCxYAW) 28 | std::cout << "use acc & yaw!" << std::endl; 29 | else if (control == MPL::JRKxYAW) 30 | std::cout << "use jrk & yaw!" << std::endl; 31 | else if (control == MPL::SNPxYAW) 32 | std::cout << "use snp & yaw!" << std::endl; 33 | else 34 | std::cout << "use null!" << std::endl; 35 | } 36 | 37 | template struct Waypoint<2>; 38 | template struct Waypoint<3>; 39 | 40 | } // namespace MPL 41 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/msckf_calib_forest3.yaml: -------------------------------------------------------------------------------- 1 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2 | 0.0, 1.0] 3 | cam0: 4 | T_cam_imu: [-0.99991370721, 0.00669023713, 0.01130570059, 0.09153893768, 5 | 0.00673396934, 0.99996997458, 0.00383452572, 0.01258894695, 6 | -0.01127970725, 0.00391032707, -0.99992873623, 0.01285669899, 7 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 8 | camera_model: pinhole 9 | intrinsics: [1064.24, 1064.45, 642.115, 343.519] 10 | distortion_model: equidistant 11 | distortion_coeffs: [-0.0958773, 0.0214831, -0.04858, 0.0374541] 12 | resolution: [1280, 800] 13 | rostopic: /ovc/left/image_raw 14 | cam1: 15 | T_cam_imu: [-0.99990916948, 0.00683919491, 0.01161370722, -0.02806080568, 16 | 0.00682247088, 0.99997563288, -0.00147903400, 0.01276787231, 17 | -0.01162353963, -0.00139966548, -0.99993146478, 0.01218006200, 18 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 19 | T_cn_cnm1: [ 0.99999994146, 0.00015016493, -0.00030745339, -0.11959767559, 20 | -0.00014852956, 0.99998587911, 0.00531220491, 0.00012440195, 21 | 0.00030824676, -0.00531215893, 0.99998584288, -0.00063779707, 22 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 23 | camera_model: pinhole 24 | intrinsics: [1060.88, 1061.18, 663.727, 350.506] 25 | distortion_model: equidistant 26 | distortion_coeffs: [-0.094406, 0.00757889, -0.0238515, 0.0216198] 27 | resolution: [1280, 800] 28 | rostopic: /ovc/right/image_raw 29 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/px4.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 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/msckf_calib_forest2.yaml: -------------------------------------------------------------------------------- 1 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2 | 0.0, 1.0] 3 | cam0: 4 | T_cam_imu: [-0.99991370721, 0.00669023713, 0.01130570059, 0.09153893768, 5 | 0.00673396934, 0.99996997458, 0.00383452572, 0.01258894695, 6 | -0.01127970725, 0.00391032707, -0.99992873623, 0.01285669899, 7 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 8 | camera_model: pinhole 9 | intrinsics: [1064.35, 1064.58, 641.491, 343.769] 10 | distortion_model: equidistant 11 | distortion_coeffs: [-0.0951089, 0.0168054, -0.0355364, 0.0261221] 12 | resolution: [1280, 800] 13 | rostopic: /ovc/left/image_raw 14 | cam1: 15 | T_cam_imu: [-0.99990295268, 0.00678173876, 0.01216935625, -0.02826749066, 16 | 0.00675825874, 0.99997522315, -0.00196952591, 0.01273644591, 17 | -0.01218241155, -0.00188709111, -0.99992401098, 0.01213942892, 18 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 19 | T_cn_cnm1: [ 0.99999962281, 0.00009488301, -0.00086335762, -0.11979648837, 20 | -0.00008987105, 0.99998315618, 0.00580338455, 0.00008132533, 21 | 0.00086389372, -0.00580330477, 0.99998278752, -0.00072307120, 22 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 23 | camera_model: pinhole 24 | intrinsics: [1060.92, 1061.23, 664.438, 350.755] 25 | distortion_model: equidistant 26 | distortion_coeffs: [-0.0919963, -0.0102372, 0.0238748, -0.0181641] 27 | resolution: [1280, 800] 28 | rostopic: /ovc/right/image_raw 29 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/msckf_calib_forest_1.yaml: -------------------------------------------------------------------------------- 1 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2 | 0.0, 1.0] 3 | cam0: 4 | T_cam_imu: [-0.99991370721, 0.00669023713, 0.01130570059, 0.09153893768, 5 | 0.00673396934, 0.99996997458, 0.00383452572, 0.01258894695, 6 | -0.01127970725, 0.00391032707, -0.99992873623, 0.01285669899, 7 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 8 | camera_model: pinhole 9 | intrinsics: [1065.33, 1065.34, 641.541, 343.254] 10 | distortion_model: equidistant 11 | distortion_coeffs: [-0.0939543, -0.000295917, 0.0137482, -0.0215914] 12 | resolution: [1280, 800] 13 | rostopic: /ovc/left/image_raw 14 | cam1: 15 | T_cam_imu: [-0.99989162305, 0.00684496103, 0.01303413488, -0.02844679190, 16 | 0.00682980510, 0.99997594823, -0.00120694513, 0.01277009414, 17 | -0.01304208288, -0.00111779372, -0.99991432364, 0.01220557040, 18 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 19 | T_cn_cnm1: [0.99999849404, 0.00016149570, -0.00172795520, -0.11996540899, 20 | -0.00015278488, 0.99998728718, 0.00504005393, 0.00013049454, 21 | 0.00172874718, -0.00503978234, 0.99998580591, -0.00074574823, 22 | 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 23 | camera_model: pinhole 24 | intrinsics: [1062.09, 1062.23, 664.454, 350.201] 25 | distortion_model: equidistant 26 | distortion_coeffs: [-0.0950587, -0.000210299, 0.0116423, -0.0183844] 27 | resolution: [1280, 800] 28 | rostopic: /ovc/right/image_raw 29 | -------------------------------------------------------------------------------- /autonomy_core/estimation/fla_ukf/launch/ukf.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 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/launch/fake_lidar_cloud.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 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(action_planner) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(Boost REQUIRED COMPONENTS timer) 6 | find_package( 7 | catkin REQUIRED 8 | COMPONENTS roscpp 9 | genmsg 10 | actionlib_msgs 11 | actionlib 12 | eigen_conversions 13 | tf2_ros 14 | tf_conversions 15 | pcl_ros 16 | mapper 17 | jps3d 18 | kr_planning_rviz_plugins 19 | kr_planning_msgs 20 | traj_opt_ros 21 | dynamic_reconfigure 22 | motion_primitive_library) 23 | 24 | generate_dynamic_reconfigure_options(cfg/ActionPlanner.cfg) 25 | 26 | catkin_package(CATKIN_DEPENDS dynamic_reconfigure kr_planning_msgs) 27 | 28 | add_library(${PROJECT_NAME} src/data_conversions.cpp src/primitive_ros_utils.cpp) 29 | target_include_directories(${PROJECT_NAME} PUBLIC src ${catkin_INCLUDE_DIRS}) 30 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) 31 | 32 | add_executable(global_plan_server src/global_plan_server.cpp) 33 | target_link_libraries(global_plan_server PUBLIC ${PROJECT_NAME} Boost::timer) 34 | add_dependencies(global_plan_server ${${PROJECT_NAME}_EXPORTED_TARGETS} 35 | ${catkin_EXPORTED_TARGETS}) 36 | 37 | add_executable(local_plan_server src/local_plan_server.cpp) 38 | target_link_libraries(local_plan_server PUBLIC ${PROJECT_NAME} Boost::timer) 39 | add_dependencies(local_plan_server ${${PROJECT_NAME}_EXPORTED_TARGETS} 40 | ${catkin_EXPORTED_TARGETS}) 41 | -------------------------------------------------------------------------------- /.github/workflows/docker-build-sim.yaml: -------------------------------------------------------------------------------- 1 | name: docker-build-sim 2 | 3 | # Build sim when "state_machine" changes, or when any files in the 4 | # autonomy_sim folder are modified 5 | on: 6 | workflow_dispatch: 7 | workflow_run: 8 | workflows: ["docker-build-state-machine"] 9 | branches: [master] 10 | types: 11 | - completed 12 | push: 13 | paths: 14 | - 'autonomy_sim/**' 15 | branches: [master] 16 | # DO NOT add a schedule. This build will be triggered by base schedule. 17 | 18 | jobs: 19 | main: 20 | runs-on: ubuntu-20.04 21 | 22 | steps: 23 | - 24 | # This action is now required because we are building with context, 25 | # required to clone a third party repo 26 | name: Checkout 27 | uses: actions/checkout@v2 28 | - 29 | name: Set up QEMU 30 | uses: docker/setup-qemu-action@v1 31 | - 32 | name: Set up Docker Buildx 33 | uses: docker/setup-buildx-action@v1 34 | - 35 | name: Login to DockerHub 36 | uses: docker/login-action@v1 37 | with: 38 | username: ${{ secrets.DOCKERHUB_USERNAME }} 39 | password: ${{ secrets.DOCKERHUB_TOKEN }} 40 | - 41 | name: Build and push 42 | id: docker_build 43 | uses: docker/build-push-action@v2 44 | with: 45 | context: . 46 | push: true 47 | tags: kumarrobotics/autonomy:sim 48 | file: ./autonomy_sim/Dockerfile 49 | platforms: linux/amd64,linux/arm64 50 | - 51 | name: Image digest 52 | run: echo ${{ steps.docker_build.outputs.digest }} 53 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/old-launch/sensing_only-old.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 | -------------------------------------------------------------------------------- /.github/workflows/docker-build-client.yaml: -------------------------------------------------------------------------------- 1 | name: docker-build-client 2 | 3 | # Build client when "map-plan" changes, or when any files in the client folder 4 | # are modified 5 | on: 6 | workflow_dispatch: 7 | workflow_run: 8 | workflows: ["docker-build-map-plan"] 9 | branches: [master] 10 | types: 11 | - completed 12 | push: 13 | paths: 14 | - 'autonomy_core/client/**' 15 | branches: [master] 16 | # DO NOT add a schedule. This build will be triggered by base schedule. 17 | 18 | jobs: 19 | main: 20 | runs-on: ubuntu-20.04 21 | 22 | steps: 23 | - 24 | # This action is now required because we are building with context, 25 | # required to clone a third party repo 26 | name: Checkout 27 | uses: actions/checkout@v2 28 | - 29 | name: Set up QEMU 30 | uses: docker/setup-qemu-action@v1 31 | - 32 | name: Set up Docker Buildx 33 | uses: docker/setup-buildx-action@v1 34 | - 35 | name: Login to DockerHub 36 | uses: docker/login-action@v1 37 | with: 38 | username: ${{ secrets.DOCKERHUB_USERNAME }} 39 | password: ${{ secrets.DOCKERHUB_TOKEN }} 40 | - 41 | name: Build and push 42 | id: docker_build 43 | uses: docker/build-push-action@v2 44 | with: 45 | context: . 46 | push: true 47 | tags: kumarrobotics/autonomy:client 48 | file: ./autonomy_core/client/Dockerfile 49 | platforms: linux/amd64,linux/arm64 50 | - 51 | name: Image digest 52 | run: echo ${{ steps.docker_build.outputs.digest }} 53 | -------------------------------------------------------------------------------- /.github/workflows/docker-build-control.yaml: -------------------------------------------------------------------------------- 1 | name: docker-build-control 2 | 3 | # Build control when "base" changes, or when any files in the control folder are 4 | # modified 5 | on: 6 | workflow_run: 7 | workflows: ["docker-build-base"] 8 | branches: [master] 9 | types: 10 | - completed 11 | workflow_dispatch: 12 | push: 13 | paths: 14 | - 'autonomy_core/control/**' 15 | branches: [master] 16 | # DO NOT add a schedule. This build will be triggered by base schedule. 17 | 18 | jobs: 19 | main: 20 | runs-on: ubuntu-20.04 21 | 22 | steps: 23 | - 24 | # This action is now required because we are building with context, 25 | # required to clone a third party repo 26 | name: Checkout 27 | uses: actions/checkout@v2 28 | - 29 | name: Set up QEMU 30 | uses: docker/setup-qemu-action@v1 31 | - 32 | name: Set up Docker Buildx 33 | uses: docker/setup-buildx-action@v1 34 | - 35 | name: Login to DockerHub 36 | uses: docker/login-action@v1 37 | with: 38 | username: ${{ secrets.DOCKERHUB_USERNAME }} 39 | password: ${{ secrets.DOCKERHUB_TOKEN }} 40 | - 41 | name: Build and push 42 | id: docker_build 43 | uses: docker/build-push-action@v2 44 | with: 45 | context: . 46 | push: true 47 | tags: kumarrobotics/autonomy:control 48 | file: ./autonomy_core/control/Dockerfile 49 | platforms: linux/amd64,linux/arm64 50 | - 51 | name: Image digest 52 | run: echo ${{ steps.docker_build.outputs.digest }} 53 | -------------------------------------------------------------------------------- /.github/workflows/docker-build-map-plan.yaml: -------------------------------------------------------------------------------- 1 | name: docker-build-map-plan 2 | 3 | # Build map-plan when "control" changes, or when any files in the map_plan 4 | # folder are modified 5 | on: 6 | workflow_dispatch: 7 | workflow_run: 8 | workflows: ["docker-build-control"] 9 | branches: [master] 10 | types: 11 | - completed 12 | push: 13 | paths: 14 | - 'autonomy_core/map_plan/**' 15 | branches: [master] 16 | # DO NOT add a schedule. This build will be triggered by base schedule. 17 | 18 | jobs: 19 | main: 20 | runs-on: ubuntu-20.04 21 | 22 | steps: 23 | - 24 | # This action is now required because we are building with context, 25 | # required to clone a third party repo 26 | name: Checkout 27 | uses: actions/checkout@v2 28 | - 29 | name: Set up QEMU 30 | uses: docker/setup-qemu-action@v1 31 | - 32 | name: Set up Docker Buildx 33 | uses: docker/setup-buildx-action@v1 34 | - 35 | name: Login to DockerHub 36 | uses: docker/login-action@v1 37 | with: 38 | username: ${{ secrets.DOCKERHUB_USERNAME }} 39 | password: ${{ secrets.DOCKERHUB_TOKEN }} 40 | - 41 | name: Build and push 42 | id: docker_build 43 | uses: docker/build-push-action@v2 44 | with: 45 | context: . 46 | push: true 47 | tags: kumarrobotics/autonomy:map_plan 48 | file: ./autonomy_core/map_plan/Dockerfile 49 | platforms: linux/amd64,linux/arm64 50 | - 51 | name: Image digest 52 | run: echo ${{ steps.docker_build.outputs.digest }} 53 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/util/convert_multicam_calib_to_msckf.py: -------------------------------------------------------------------------------- 1 | import ruamel.yaml 2 | import copy 3 | 4 | # TODO: maybe make those as input args 5 | in_folder_name = "/home/dcist/multicam_calibration/calib/example/" 6 | out_folder_name = "../config/" 7 | input_fname = 'ovc-latest.yaml' 8 | output_fname = 'msckf_calib.yaml' 9 | output_calib = None 10 | 11 | with open(in_folder_name + input_fname, 'r') as file: 12 | 13 | loaded_calib = ruamel.yaml.safe_load(file) 14 | output_calib = copy.deepcopy(loaded_calib) 15 | 16 | # add T_imu_body as identity transform 17 | output_calib['T_imu_body'] = [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 18 | 0.0, 1.0] 19 | 20 | # remove unused data 21 | del output_calib['cam0']['rectification_matrix'] 22 | del output_calib['cam0']['projection_matrix'] 23 | del output_calib['cam1']['rectification_matrix'] 24 | del output_calib['cam1']['projection_matrix'] 25 | del output_calib['cam1']['T_cam_imu'] 26 | 27 | # modify the T_cam_imu and T_cn_cnm1 format 28 | output_calib['cam0']['T_cam_imu'] = [] 29 | output_calib['cam1']['T_cn_cnm1'] = [] 30 | 31 | for i in range(4): 32 | for j in range(4): 33 | output_calib['cam0']['T_cam_imu'].append(loaded_calib['cam0']['T_cam_imu'][i][j]) 34 | output_calib['cam1']['T_cn_cnm1'].append(loaded_calib['cam1']['T_cn_cnm1'][i][j]) 35 | if j != 3: 36 | output_calib['cam0']['T_cam_imu'][i].append(',') 37 | output_calib['cam1']['T_cn_cnm1'][i].append(',') 38 | 39 | with open(out_folder_name + output_fname, 'w') as file: 40 | ruamel.yaml.safe_dump(output_calib, file) 41 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/include/traj_opt_ros/traj_data.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Michael Watterson 2 | #pragma once 3 | 4 | #include 5 | #include 6 | 7 | namespace traj_opt { 8 | 9 | enum PolyType { 10 | STANDARD, 11 | LEGENDRE, 12 | WATTERSON, 13 | BEZIER, 14 | ENDPOINT, 15 | LIU, 16 | CHEBYSHEV 17 | }; 18 | 19 | struct PolynomialData { 20 | int degree; 21 | float dt; 22 | PolyType basis; 23 | std::vector coeffs; 24 | // polynomials are stored with parameterization s \in [0,1] 25 | // time duration dt is used to evaluate polynomial p(t/dt) for t \in [0,dt] 26 | 27 | // define with and without space allocating constructor 28 | PolynomialData() {} 29 | explicit PolynomialData(int d) : degree(d), coeffs(degree + 1) {} 30 | }; 31 | 32 | struct SplineData { 33 | int segments; 34 | float t_total; 35 | // t_total should equal the sum of dt for each segment 36 | std::vector segs; 37 | 38 | // define with and without space allocating constructor 39 | SplineData() {} 40 | SplineData(int deg, int seg) 41 | : segments(seg), segs(segments, PolynomialData(deg)) {} 42 | }; 43 | 44 | struct TrajData { 45 | int dimensions; 46 | std::vector data; 47 | std::vector dimension_names; 48 | // dimension_names are optional, but are useful for determining what this 49 | // trajectory parametrizes 50 | // ex. dimension_names = {'x','y',z'} 51 | 52 | // define with and without space allocating constructor 53 | TrajData() {} 54 | TrajData(int dim, int segs, int deg) 55 | : dimensions(dim), data(dimensions, SplineData(deg, segs)) {} 56 | }; 57 | 58 | } // namespace traj_opt 59 | -------------------------------------------------------------------------------- /.github/workflows/docker-build-state-machine.yaml: -------------------------------------------------------------------------------- 1 | name: docker-build-state-machine 2 | 3 | # Build state-machine when "map-plan" changes, or when any files in the 4 | # state_machine folder are modified 5 | on: 6 | workflow_dispatch: 7 | workflow_run: 8 | workflows: ["docker-build-map-plan"] 9 | branches: [master] 10 | types: 11 | - completed 12 | push: 13 | paths: 14 | - 'autonomy_core/state_machine/**' 15 | branches: [master] 16 | # DO NOT add a schedule. This build will be triggered by base schedule. 17 | 18 | jobs: 19 | main: 20 | runs-on: ubuntu-20.04 21 | 22 | steps: 23 | - 24 | # This action is now required because we are building with context, 25 | # required to clone a third party repo 26 | name: Checkout 27 | uses: actions/checkout@v2 28 | - 29 | name: Set up QEMU 30 | uses: docker/setup-qemu-action@v1 31 | - 32 | name: Set up Docker Buildx 33 | uses: docker/setup-buildx-action@v1 34 | - 35 | name: Login to DockerHub 36 | uses: docker/login-action@v1 37 | with: 38 | username: ${{ secrets.DOCKERHUB_USERNAME }} 39 | password: ${{ secrets.DOCKERHUB_TOKEN }} 40 | - 41 | name: Build and push 42 | id: docker_build 43 | uses: docker/build-push-action@v2 44 | with: 45 | context: . 46 | push: true 47 | tags: kumarrobotics/autonomy:state_machine 48 | file: ./autonomy_core/state_machine/Dockerfile 49 | platforms: linux/amd64,linux/arm64 50 | - 51 | name: Image digest 52 | run: echo ${{ steps.docker_build.outputs.digest }} 53 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_core/src/traj_opt_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace state_machine { 4 | 5 | traj_opt::VecD Make4d(const traj_opt::VecD& vec) { 6 | traj_opt::VecD out = traj_opt::VecD::Zero(4); 7 | long int rows = std::min(vec.rows(), static_cast(4)); 8 | out.block(0, 0, rows, 1) = vec.block(0, 0, rows, 1); 9 | return out; 10 | } 11 | 12 | void VecToPose(const traj_opt::VecD& valn, geometry_msgs::Pose* pos) { 13 | if (pos != NULL) { 14 | traj_opt::VecD val = Make4d(valn); 15 | pos->position.x = val(0), pos->position.y = val(1), 16 | pos->position.z = val(2); 17 | pos->orientation.w = std::cos(0.5 * val(3)); 18 | pos->orientation.z = std::sin(0.5 * val(3)); 19 | } 20 | } 21 | 22 | void VecToTwist(const traj_opt::VecD& valn, geometry_msgs::Twist* vel) { 23 | if (vel != NULL) { 24 | traj_opt::VecD val = Make4d(valn); 25 | vel->linear.x = val(0), vel->linear.y = val(1), vel->linear.z = val(2); 26 | vel->angular.z = val(3); 27 | } 28 | } 29 | 30 | void EvaluateToMsgs(boost::shared_ptr traj, 31 | double dt, 32 | geometry_msgs::Pose* pos, 33 | geometry_msgs::Twist* vel, 34 | geometry_msgs::Twist* acc, 35 | geometry_msgs::Twist* jrk) { 36 | if (traj == NULL) return; 37 | traj_opt::VecD val; 38 | traj->evaluate(dt, 0, val); 39 | VecToPose(val, pos); 40 | 41 | traj->evaluate(dt, 1, val); 42 | VecToTwist(val, vel); 43 | 44 | traj->evaluate(dt, 2, val); 45 | VecToTwist(val, acc); 46 | 47 | traj->evaluate(dt, 3, val); 48 | VecToTwist(val, jrk); 49 | } 50 | 51 | } // namespace state_machine 52 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/fake_lidar/launch/fake_lidar_semantic_cloud.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 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | #### rviz #### 3 | 7 | 8 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 39 | 40 | 41 | 42 | #### current odom visual #### 43 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/rosflight.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /autonomy_core/estimation/estimation_launch/config/msckf_calib_simulation.yaml: -------------------------------------------------------------------------------- 1 | # The modifications of the output file from Kalibr: 2 | # 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix. 3 | # 2. Add the T_imu_body at the end of the calibration file (usually set to identity). 4 | T_imu_body: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 5 | 0.0, 1.0] 6 | 7 | cam0: 8 | T_cam_imu: [-2.180922109573658e-12, -1.000000000002153, -2.384185790460513e-07, 0.09999827997284427, 1.110223024625157e-16, 2.384163980684306e-07, -1.000000000002153, -1.76864896894724e-08, 1.000000000002181, -1.110223024625157e-16, -2.180922109573658e-12, -0.09999866653912193, 0, 0, 0, 1] 9 | camera_model: pinhole 10 | intrinsics: [173.9987021397093, 173.9987021397093, 240, 160] 11 | distortion_model: radtan 12 | distortion_coeffs: [0, 0, 0, 0] 13 | resolution: [480, 320] 14 | rostopic: /quadrotor/cam_left/image_raw 15 | cam1: 16 | T_cam_imu: [-2.181366198783508e-12, -1.000000000002153, -2.384185790460513e-07, -0.09999866826978376, -1.110223024625157e-16, 2.38416397846386e-07, -1.000000000002153, 3.044861815459932e-08, 1.000000000002181, 0, -2.181366198783508e-12, -0.09999827910891668, 0, 0, 0, 1] 17 | camera_model: pinhole 18 | intrinsics: [173.9987021397093, 173.9987021397093, 240, 160] 19 | distortion_model: radtan 20 | distortion_coeffs: [0, 0, 0, 0] 21 | resolution: [480, 320] 22 | rostopic: /quadrotor/cam_right/image_raw 23 | T_cn_cnm1: [1.000000000004362, 2.18097762061901e-12, -2.181254656513452e-12, -0.1999969482432824, 2.181199665382753e-12, 1.000000000004362, 2.18081108724942e-12, 4.813510780613475e-08, -2.180921589500185e-12, 2.181477221090666e-12, 1.000000000004362, 3.874308595469556e-07, 0, 0, 0, 1] -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/src/primitive_ros_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file primitive_ros_utils.h 3 | * @brief Interface between primitive classes and ROS 4 | */ 5 | 6 | #pragma once 7 | #include 8 | #include 9 | #include 10 | 11 | /// Primitive2D to primitive ROS message 12 | kr_planning_msgs::Primitive toPrimitiveROSMsg(const MPL::Primitive2D &pr, 13 | double z = 0); 14 | 15 | /// Primitive3D to primitive ROS message 16 | kr_planning_msgs::Primitive toPrimitiveROSMsg(const MPL::Primitive3D &pr); 17 | 18 | /// Multiple Primitive2D to Primitive array ROS message 19 | kr_planning_msgs::PrimitiveArray toPrimitiveArrayROSMsg( 20 | const vec_E &prs, double z = 0); 21 | 22 | /// Multiple Primitive3D to Primitive array ROS message 23 | kr_planning_msgs::PrimitiveArray toPrimitiveArrayROSMsg( 24 | const vec_E &prs); 25 | 26 | /// Trajectory2D class to trajectory ROS message 27 | kr_planning_msgs::Trajectory toTrajectoryROSMsg(const MPL::Trajectory2D &traj, 28 | double z = 0); 29 | 30 | /// Trajectory3D class to trajectory ROS message 31 | kr_planning_msgs::Trajectory toTrajectoryROSMsg(const MPL::Trajectory3D &traj); 32 | 33 | /// ROS message to Primitive2D class 34 | MPL::Primitive2D toPrimitive2D(const kr_planning_msgs::Primitive &pr); 35 | 36 | /// ROS message to Primitive3D class 37 | MPL::Primitive3D toPrimitive3D(const kr_planning_msgs::Primitive &pr); 38 | 39 | /// ROS message to Trajectory2D class 40 | MPL::Trajectory2D toTrajectory2D(const kr_planning_msgs::Trajectory &traj_msg); 41 | 42 | /// ROS message to Trajectory3D class 43 | MPL::Trajectory3D toTrajectory3D(const kr_planning_msgs::Trajectory &traj_msg); -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/falcon4-multicam-output-2021-07-25.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | rectification_matrix: 3 | - [ 0.99998513429, -0.00072231833, 0.00540457690] 4 | - [ 0.00071000439, 0.99999714874, 0.00228000146] 5 | - [-0.00540620838, -0.00227613029, 0.99998279592] 6 | projection_matrix: 7 | - [1016.28949887392, 0.00000000000, 652.56153679049, 0.00000000000] 8 | - [ 0.00000000000, 1016.28949887392, 329.65089242489, 0.00000000000] 9 | - [ 0.00000000000, 0.00000000000, 1.00000000000, 0.00000000000] 10 | camera_model: pinhole 11 | intrinsics: [1067.96, 1068.1, 641.371, 343.293] 12 | distortion_model: equidistant 13 | distortion_coeffs: [-0.0964802, 0.00443556, 0.00765515, -0.0180741] 14 | resolution: [1280, 800] 15 | rostopic: /ovc/left/image_raw 16 | cam1: 17 | T_cn_cnm1: 18 | - [ 0.99999976816, 0.00010526340, -0.00067275668, -0.11983986808] 19 | - [-0.00010219715, 0.99998961548, 0.00455614903, 0.00009585961] 20 | - [ 0.00067322929, -0.00455607922, 0.99998939440, -0.00072876351] 21 | - [ 0.00000000000, 0.00000000000, 0.00000000000, 1.00000000000] 22 | rectification_matrix: 23 | - [ 0.99998119046, -0.00079988241, 0.00608102979] 24 | - [ 0.00081373344, 0.99999707971, -0.00227561086] 25 | - [-0.00607919181, 0.00228051640, 0.99997892111] 26 | projection_matrix: 27 | - [1016.28949887392, 0.00000000000, 652.56153679049, -121.79429175304] 28 | - [ 0.00000000000, 1016.28949887392, 329.65089242489, 0.00000000000] 29 | - [ 0.00000000000, 0.00000000000, 1.00000000000, 0.00000000000] 30 | camera_model: pinhole 31 | intrinsics: [1064.59, 1064.79, 664.657, 350.615] 32 | distortion_model: equidistant 33 | distortion_coeffs: [-0.0963886, -0.00104029, 0.0133809, -0.017335] 34 | resolution: [1280, 800] 35 | rostopic: /ovc/right/image_raw 36 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/traj_opt_ros/src/trajectory.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Michael Watterson 2 | #include "traj_opt_ros/trajectory.h" 3 | 4 | namespace traj_opt { 5 | 6 | double Trajectory::getExecuteTime() const { 7 | if (exec_t <= 0) 8 | return getTotalTime(); 9 | else 10 | return exec_t; 11 | } 12 | 13 | bool Trajectory::getCommand(double t, uint num_derivatives, MatD &data) { 14 | // check input 15 | if (dim_ < 1) return false; 16 | 17 | // allocate data 18 | data = MatD::Zero(dim_, num_derivatives + 1); 19 | // evaluate and pack 20 | for (uint i = 0; i <= num_derivatives; i++) { 21 | VecD block; 22 | this->evaluate(t, i, block); 23 | data.block(0, i, dim_, 1) = block; 24 | } 25 | return true; 26 | } 27 | 28 | bool Trajectory::getHopfChart(double t) { 29 | VecD val; 30 | evaluate(t, 0, val); 31 | if (val.rows() < 5) return true; 32 | 33 | return val(4) < 0.5; 34 | } 35 | 36 | std::pair Trajectory::getYaws(double t) { 37 | VecD pos, vel; 38 | evaluate(t, 0, pos); 39 | evaluate(t, 1, vel); 40 | 41 | if (pos.rows() < 4) 42 | return std::make_pair(0.0, 0.0); 43 | else 44 | return std::make_pair(pos(3), vel(3)); 45 | } 46 | std::pair Trajectory::getXi(double t, double g) { 47 | VecD gv = VecD::Zero(3, 1); 48 | gv(2) = g; 49 | VecD evalv, evald; 50 | evaluate(t, 2, evalv); 51 | evaluate(t, 3, evald); 52 | VecD eval3 = evalv.block<3, 1>(0, 0); 53 | VecD evald3 = evald.block<3, 1>(0, 0); 54 | 55 | VecD xi = eval3 + gv; 56 | xi.normalize(); 57 | // calc differential 58 | VecD xid = xi.dot(xi) * evald3; 59 | xid -= xi.dot(evald3) * xi; 60 | xid /= std::pow(xi.norm(), 3.0); 61 | 62 | Vec3 xi3, xid3; 63 | xi3 = eval3.head(3) + gv.head(3); 64 | xid3 = evald3.head(3); 65 | return std::make_pair(xi3, xid); 66 | } 67 | 68 | } // namespace traj_opt 69 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/coverage_utils/src/coverage_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // Timer stuff 12 | using boost::timer::cpu_timer; 13 | using boost::timer::cpu_times; 14 | 15 | CoveragePlanner::CoveragePlanner() : nh_("/"), pnh_("~") { 16 | initial_polygon_publisher_ = 17 | nh_.advertise("original_polygon", 1); 18 | path_pub_ = nh_.advertise("coverage_path", 1, true); 19 | all_points_pub_ = 20 | nh_.advertise("all_input_points", 1, true); 21 | pnh_.param("input_file_path", fname_, std::string("input.txt")); 22 | } 23 | 24 | void CoveragePlanner::RunPlanner() { 25 | // load all points 26 | auto pt_vec = PreprocessData(fname_); 27 | 28 | std::vector::iterator it; 29 | vec_Vec3f all_points; 30 | for (it = pt_vec.begin(); it != pt_vec.end(); it++) { 31 | all_points.push_back(Vec3f(it->x, it->y, 5)); 32 | } 33 | 34 | // convex hull of all points 35 | convex_hull(&pt_vec); 36 | geometry_msgs::PolygonStamped stamped_poly; 37 | stamped_poly.header.frame_id = "map"; 38 | stamped_poly.header.stamp = ros::Time::now(); 39 | for (it = pt_vec.begin(); it != pt_vec.end(); it++) { 40 | geometry_msgs::Point32 point; 41 | point.x = static_cast(it->x); 42 | point.y = static_cast(it->y); 43 | point.z = static_cast(0.0); 44 | stamped_poly.polygon.points.push_back(point); 45 | } 46 | 47 | auto all_points_msg = path_to_ros(all_points); 48 | all_points_msg.header.frame_id = "map"; 49 | all_points_pub_.publish(all_points_msg); 50 | 51 | initial_polygon_publisher_.publish(stamped_poly); 52 | } 53 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/old-launch/msckf_only.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 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/mpl/include/mpl_basis/math.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file math.h 3 | * @brief Polynomial roots solver 4 | 5 | * Solving real roots for n-th order polynomial: 6 | if n < 5, the closed form solution will be calculated; 7 | if n >= 5, using Eigen Polynomials solver which is slower but correct. 8 | */ 9 | #pragma once 10 | #include 11 | 12 | #include "mpl_basis/data_type.h" 13 | 14 | namespace MPL { 15 | 16 | decimal_t normalize_angle(decimal_t angle); 17 | 18 | /// Quadratic equation: \f$b*t^2+c*t+d = 0\f$ 19 | std::vector quad(decimal_t b, decimal_t c, decimal_t d); 20 | 21 | /// Cubic equation: \f$a*t^3+b*t^2+c*t+d = 0\f$ 22 | std::vector cubic(decimal_t a, decimal_t b, decimal_t c, 23 | decimal_t d); 24 | 25 | /// Quartic equation: \f$a*t^4+b*t^3+c*t^2+d*t+e = 0\f$ 26 | std::vector quartic(decimal_t a, decimal_t b, decimal_t c, 27 | decimal_t d, decimal_t e); 28 | 29 | /*! \brief General solver for \f$a*t^4+b*t^3+c*t^2+d*t+e = 0\f$ 30 | 31 | \f$a, b, c\f$ can be zero. The function itself checks the highest order of the 32 | polynomial. 33 | */ 34 | std::vector solve(decimal_t a, decimal_t b, decimal_t c, decimal_t d, 35 | decimal_t e); 36 | 37 | /// General solver for \f$a*t^5+b*t^4+c*t^3+d*t^2+e*t+f = 0\f$ 38 | std::vector solve(decimal_t a, decimal_t b, decimal_t c, decimal_t d, 39 | decimal_t e, decimal_t f); 40 | 41 | /// General solver for \f$a*t^6+b*t^5+c*t^4+d*t^3+e*t^2+f*t+g = 0\f$ 42 | std::vector solve(decimal_t a, decimal_t b, decimal_t c, decimal_t d, 43 | decimal_t e, decimal_t f, decimal_t g); 44 | 45 | /// Return \f$n!\f$ 46 | // int factorial(int n); 47 | 48 | /// Return \f$t^n\f$ 49 | decimal_t power(decimal_t t, int n); 50 | 51 | } // namespace MPL 52 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/map_plan_launch/launch/map_plan.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 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/sim/full_sim_jetson_nuc.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 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/config/calib_july_aug_2021/kalibr-8-30-2021/result-1st-success/camchain-imucam-.calibration-8-30-2021cam-imu.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | T_cam_imu: 3 | - [-0.9999986418189603, 0.0006908622162476218, 0.0014963521087460437, 0.09376659024554503] 4 | - [0.0006963388591197592, 0.9999930503125286, 0.003662572707558209, 0.014339979084646638] 5 | - [-0.0014938113764686287, 0.003663609701241646, -0.999992173215135, -0.010004665255030111] 6 | - [0.0, 0.0, 0.0, 1.0] 7 | cam_overlaps: [1] 8 | camera_model: pinhole 9 | distortion_coeffs: [-0.08921669100012515, 0.0036755424278144865, -0.011708374532048397, 10 | 0.005799357286396118] 11 | distortion_model: equidistant 12 | intrinsics: [1058.04703898802, 1058.2196100463393, 642.0867566020861, 343.6212525674095] 13 | resolution: [1280, 800] 14 | rostopic: /ovc/left/image_raw 15 | timeshift_cam_imu: 0.008447399975430408 16 | cam1: 17 | T_cam_imu: 18 | - [-0.9999912909755342, 0.0009044045769987138, 0.004074312880177525, -0.02618037855001458] 19 | - [0.0009049615183905016, 0.9999995814293963, 0.00013485430135382585, 0.014344939592096578] 20 | - [-0.004074189211942549, 0.00013854022327485947, -0.9999916908599151, -0.011096417434765787] 21 | - [0.0, 0.0, 0.0, 1.0] 22 | T_cn_cnm1: 23 | - [0.9999966542319435, 0.0002229879641389485, -0.002577169239196196, -0.11997563643258276] 24 | - [-0.00021389657269771451, 0.999993755818389, 0.0035274030800726635, 6.0396888206409554e-05] 25 | - [0.002577939715314946, -0.003526840030532588, 0.9999904577675849, -0.0012829974512676998] 26 | - [0.0, 0.0, 0.0, 1.0] 27 | cam_overlaps: [0] 28 | camera_model: pinhole 29 | distortion_coeffs: [-0.09337087156090869, 0.026346618183340113, -0.06329486672350314, 30 | 0.04644342995151147] 31 | distortion_model: equidistant 32 | intrinsics: [1055.434094454324, 1055.6845447978794, 666.9265866693742, 351.85682800429487] 33 | resolution: [1280, 800] 34 | rostopic: /ovc/right/image_raw 35 | timeshift_cam_imu: 0.009365721693268306 36 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(state_machine) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(Eigen3) 6 | find_package(Boost REQUIRED COMPONENTS timer) 7 | find_package( 8 | catkin REQUIRED 9 | COMPONENTS kr_mav_msgs 10 | kr_planning_rviz_plugins 11 | traj_opt_ros 12 | kr_planning_msgs 13 | actionlib 14 | sensor_msgs 15 | actionlib_msgs 16 | action_planner 17 | tf2_ros 18 | action_trackers 19 | geometry_msgs 20 | eigen_conversions 21 | tf_conversions) 22 | 23 | add_action_files(DIRECTORY action FILES Replan.action) 24 | generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs) 25 | 26 | catkin_package(CATKIN_DEPENDS actionlib_msgs geometry_msgs) 27 | 28 | add_library(${PROJECT_NAME} src/traj_opt_utils.cpp) 29 | target_include_directories(${PROJECT_NAME} PUBLIC include 30 | ${catkin_INCLUDE_DIRS}) 31 | target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) 32 | 33 | add_executable(local_global_replan_server src/local_global_replan_server.cpp) 34 | target_include_directories(local_global_replan_server 35 | PUBLIC include ${catkin_INCLUDE_DIRS}) 36 | target_link_libraries( 37 | local_global_replan_server PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES} 38 | Boost::timer Eigen3::Eigen) 39 | add_dependencies(local_global_replan_server ${${PROJECT_NAME}_EXPORTED_TARGETS} 40 | ${catkin_EXPORTED_TARGETS}) 41 | 42 | # JPS + line tracker based replanner, no longer used since we are using motion primitive planner 43 | # add_executable(path_replanner src/path_replanner.cpp) 44 | # target_link_libraries(path_replanner PUBLIC ${PROJECT_NAME}) 45 | # add_dependencies(path_replanner ${${PROJECT_NAME}_EXPORTED_TARGETS} 46 | # ${catkin_EXPORTED_TARGETS}) 47 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_launch/launch/system.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 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/sim/sim_quad_polypixel.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 | -------------------------------------------------------------------------------- /autonomy_core/base/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cudagl:11.4.2-base-ubuntu20.04 2 | 3 | RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub 4 | 5 | #Run the frontend first so it doesn't throw an error later 6 | RUN export DEBIAN_FRONTEND=noninteractive \ 7 | && apt-get update \ 8 | && export TZ="America/New_York" \ 9 | && apt-get install -y keyboard-configuration tzdata \ 10 | && ln -fs "/usr/share/zoneinfo/$TZ" /etc/localtime \ 11 | && dpkg-reconfigure --frontend noninteractive tzdata \ 12 | && apt-get clean 13 | 14 | # General tools 15 | RUN apt-get update \ 16 | && apt-get install -y \ 17 | build-essential \ 18 | cmake \ 19 | cppcheck \ 20 | gdb \ 21 | git \ 22 | lsb-release \ 23 | software-properties-common \ 24 | sudo \ 25 | neovim \ 26 | wget \ 27 | net-tools \ 28 | iputils-ping \ 29 | tmux \ 30 | locales \ 31 | python3-pip \ 32 | curl \ 33 | && apt-get clean 34 | 35 | # Set locales 36 | RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && \ 37 | locale-gen 38 | ENV LC_ALL en_US.UTF-8 39 | ENV LANG en_US.UTF-8 40 | ENV LANGUAGE en_US:en 41 | 42 | # ROS Setup 43 | RUN sudo apt-get update \ 44 | && sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \ 45 | && curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - \ 46 | && sudo apt-get update \ 47 | && sudo apt-get install -y \ 48 | python3-catkin-tools \ 49 | python3-rosdep \ 50 | python3-rosinstall \ 51 | python3-vcstool \ 52 | ros-noetic-catkin \ 53 | ros-noetic-rosbash \ 54 | ros-noetic-desktop \ 55 | ros-noetic-pcl-ros \ 56 | ros-noetic-tf2-geometry-msgs \ 57 | && sudo rosdep init \ 58 | && rosdep update 59 | 60 | COPY autonomy_core/base/docker/entrypoint.sh /entrypoint.sh 61 | ENTRYPOINT ["/entrypoint.sh"] 62 | -------------------------------------------------------------------------------- /autonomy_core/interface/mavros_interface/launch/fla250.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/sim/sim_quad_overpass_customized.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 | -------------------------------------------------------------------------------- /autonomy_sim/gazebo_sim/gazebo_utils/launch/simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/ouster_decoder.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 | -------------------------------------------------------------------------------- /autonomy_core/map_plan/action_planner/cfg/ActionPlanner.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = "action_planner" 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | 7 | gen = ParameterGenerator() 8 | 9 | gen.add("degree", int_t, 0, "Polynomial Degree", 7, 5, 15) 10 | gen.add("order", int_t, 0, "Minization Order", 3, 1, 7) 11 | gen.add("samples", int_t, 0, "Constraint Samples", 10, 5, 100) 12 | gen.add("max_v", double_t, 0, "Maximum Velocity", 10, 1, 30) 13 | gen.add("max_a", double_t, 0, "Maximum Accerleration", 5, 2, 20) 14 | gen.add("max_j", double_t, 0, "Maximum Jerk", 25, 10, 200) 15 | gen.add("max_w", double_t, 0, "Maximum Angular Velocity", 1.0, 0.1, 10) 16 | 17 | 18 | poly_enum = gen.enum([ gen.const("Standard", int_t, 0, "1, t, ..."), 19 | gen.const("Legendre", int_t, 1, "see wikipedia"), 20 | gen.const("Bezier", int_t, 2, "see wikipedia"), 21 | gen.const("Chebyshev", int_t, 3, "Better than Chebychase basis"), 22 | gen.const("Endpoint", int_t, 4, "Boundary condition basis")], 23 | "Basis Types") 24 | 25 | gen.add("polytype", int_t, 0, "Choose a basis", 1, 0, 4, edit_method=poly_enum) 26 | 27 | con_enum = gen.enum([ gen.const("Control", int_t, 0, "Control Points"), 28 | gen.const("Sample", int_t, 1, "see the minimum snap paper"), 29 | gen.const("Cheby", int_t, 2, "like sample, but with Chebyshevs intervals")], 30 | "Constraint Types") 31 | 32 | gen.add("contype", int_t, 0, "Choose a basis", 0, 0, 2, edit_method=con_enum) 33 | 34 | gen.add("lx", double_t, 0, "Min X", -500, -1000, 1000) 35 | gen.add("ly", double_t, 0, "Min Y", -500, -1000, 1000) 36 | gen.add("lz", double_t, 0, "Min Z", 0, -1000, 1000) 37 | gen.add("ux", double_t, 0, "Max X", 1000, -1000, 2000) 38 | gen.add("uy", double_t, 0, "Max Y", 500, -1000, 1000) 39 | gen.add("uz", double_t, 0, "Max Z", 5, -1000, 1000) 40 | 41 | 42 | 43 | exit(gen.generate(PACKAGE, "action_planner", "ActionPlanner")) 44 | -------------------------------------------------------------------------------- /autonomy_sim/unity_sim/dcist_utils/launch/sim/sim_quad.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 | -------------------------------------------------------------------------------- /autonomy_core/state_machine/state_machine_core/scripts/Utils.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from __future__ import division 3 | from __future__ import print_function 4 | 5 | import geometry_msgs.msg as GM 6 | import tf 7 | import rospy 8 | import numpy 9 | 10 | 11 | def pose_to_transform(msg): 12 | m = GM.TransformStamped() 13 | m.header = msg.header 14 | m.transform.rotation = msg.pose.orientation 15 | m.transform.translation.x = msg.pose.position.x 16 | m.transform.translation.y = msg.pose.position.y 17 | m.transform.translation.z = msg.pose.position.z 18 | return m 19 | 20 | 21 | def odom_to_pose(msg): 22 | m = GM.PoseStamped() 23 | m.header = msg.header 24 | m.pose = msg.pose.pose 25 | return m 26 | 27 | 28 | def transform_inverse(msg): 29 | transform = GM.TransformStamped() 30 | transform.header = msg.header 31 | t = tf.Transformer(True, rospy.Duration.from_sec(0.1)) 32 | msg.header.frame_id = "a" 33 | msg.child_frame_id = "b" 34 | t.setTransform(msg) 35 | (trans, rot) = t.lookupTransform("b", "a", rospy.Time(0)) 36 | transform.transform.translation.x = trans[0] 37 | transform.transform.translation.y = trans[1] 38 | transform.transform.translation.z = trans[2] 39 | transform.transform.rotation.x = rot[0] 40 | transform.transform.rotation.y = rot[1] 41 | transform.transform.rotation.z = rot[2] 42 | transform.transform.rotation.w = rot[3] 43 | return transform 44 | 45 | 46 | def pose_err(p1, p2): # assume quaternions are only rotation about z 47 | # err = numpy.power(p1.position.x - p2.position.x, 2.0) + 48 | # numpy.power(p1.position.y - p2.position.y, 2.0) + 49 | # numpy.power(p1.position.z - p2.position.z, 2.0) + 50 | # numpy.power(numpy.abs(p1.orientation.w) - 51 | # numpy.abs(p2.orientation.w), 2.0) 52 | err = numpy.power(p1.position.x - p2.position.x, 2.0) + numpy.power( 53 | p1.position.y - p2.position.y, 2.0 54 | ) 55 | # err = numpy.power(p1.position.x - p2.position.x, 2.0) + numpy.power(p1.position.y - p2.position.y, 2.0) + numpy.power(numpy.abs(p1.orientation.w) - numpy.abs(p2.orientation.w), 2.0) 56 | return numpy.sqrt(err) 57 | -------------------------------------------------------------------------------- /autonomy_real/real_experiment_launch/launch/old-launch/estimation_only.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 | --------------------------------------------------------------------------------