diff --git a/.github/workflows/catkin_lint.yml b/.github/workflows/catkin_lint.yml
new file mode 100644
index 000000000..e4c008193
--- /dev/null
+++ b/.github/workflows/catkin_lint.yml
@@ -0,0 +1,28 @@
+on: [push, pull_request]
+
+jobs:
+  test:
+    runs-on: ubuntu-latest
+    name: catkin_lint
+
+    steps:
+      - name: Setup OS
+        run: |
+          sudo apt-get update -y
+          sudo apt-get upgrade -y
+      - name: Setup Git
+        run: |
+          sudo apt-get install -y git
+      - name: Checkout
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+      - name: catkin lint setup
+        run: |
+          sudo apt-get install -y -q python3-pip
+          sudo pip3 install catkin_lint rosdep
+          sudo rosdep init
+          rosdep update
+      - name: catkin lint test
+        run: |
+          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD
diff --git a/aerial_robot/CMakeLists.txt b/aerial_robot/CMakeLists.txt
index 191f78585..e752373bd 100644
--- a/aerial_robot/CMakeLists.txt
+++ b/aerial_robot/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot)
 find_package(catkin REQUIRED)
 catkin_metapackage()
diff --git a/aerial_robot/package.xml b/aerial_robot/package.xml
index ccc263358..0799b936d 100644
--- a/aerial_robot/package.xml
+++ b/aerial_robot/package.xml
@@ -16,12 +16,12 @@
   <exec_depend>aerial_robot_control</exec_depend>
   <exec_depend>aerial_robot_model</exec_depend>
   <exec_depend>aerial_robot_msgs</exec_depend>
-  <exec_depend>motor_test</exec_depend>
-  <exec_depend>spinal</exec_depend>
   <exec_depend>aerial_robot_simulation</exec_depend>
+  <exec_depend>dragon</exec_depend>
   <exec_depend>hydrus</exec_depend>
   <exec_depend>hydrus_xi</exec_depend>
-  <exec_depend>dragon</exec_depend>
+  <exec_depend>motor_test</exec_depend>
+  <exec_depend>spinal</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python-is-python3</exec_depend>
 
   <export>
diff --git a/aerial_robot_base/CMakeLists.txt b/aerial_robot_base/CMakeLists.txt
index c2e9f2527..72a5a4522 100644
--- a/aerial_robot_base/CMakeLists.txt
+++ b/aerial_robot_base/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot_base)
 
 
@@ -14,11 +14,12 @@ find_package(catkin REQUIRED COMPONENTS
 
 catkin_python_setup()
 
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+add_compile_options(-std=c++14)
 
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS aerial_robot_control aerial_robot_estimation aerial_robot_model roscpp rospy
 )
 
 include_directories(
@@ -31,3 +32,25 @@ target_link_libraries (aerial_robot_base ${catkin_LIBRARIES})
 
 add_executable(aerial_robot_base_node src/aerial_robot_base_node.cpp)
 target_link_libraries (aerial_robot_base_node ${catkin_LIBRARIES} aerial_robot_base)
+
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS aerial_robot_base
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(TARGETS aerial_robot_base_node
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY bin
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+install(DIRECTORY launch scripts
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
diff --git a/aerial_robot_base/package.xml b/aerial_robot_base/package.xml
index 6d5a9ac6b..d5ccf5178 100644
--- a/aerial_robot_base/package.xml
+++ b/aerial_robot_base/package.xml
@@ -25,5 +25,7 @@
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>rostest</run_depend>
+  <run_depend>ublox_gps</run_depend>
+  <run_depend>ntrip_ros</run_depend>
 
 </package>
diff --git a/aerial_robot_control/CMakeLists.txt b/aerial_robot_control/CMakeLists.txt
index 28c639826..4b49b938b 100644
--- a/aerial_robot_control/CMakeLists.txt
+++ b/aerial_robot_control/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot_control)
 
 add_compile_options(-std=c++11)
@@ -38,7 +38,7 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DNDEBUG")
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES control_utils flight_control_pluginlib flight_navigation
-  CATKIN_DEPENDS aerial_robot_estimation aerial_robot_model aerial_robot_msgs roscpp spinal
+  CATKIN_DEPENDS aerial_robot_estimation aerial_robot_model aerial_robot_msgs dynamic_reconfigure pluginlib roscpp spinal tf
   DEPENDS EIGEN3
 )
 
@@ -66,3 +66,15 @@ add_dependencies(flight_control_pluginlib  ${PROJECT_NAME}_gencfg)
 ### flight navigation
 add_library (flight_navigation src/flight_navigation.cpp)
 target_link_libraries (flight_navigation ${catkin_LIBRARIES})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS control_utils flight_control_pluginlib flight_navigation
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY scripts plugins
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
diff --git a/aerial_robot_estimation/CMakeLists.txt b/aerial_robot_estimation/CMakeLists.txt
index 75a6969c2..035607546 100644
--- a/aerial_robot_estimation/CMakeLists.txt
+++ b/aerial_robot_estimation/CMakeLists.txt
@@ -1,7 +1,7 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot_estimation)
 
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+add_compile_options(-std=c++14)
 
 find_package(catkin REQUIRED COMPONENTS
   aerial_robot_model
@@ -14,11 +14,9 @@ find_package(catkin REQUIRED COMPONENTS
   kalman_filter
   nav_msgs
   nodelet
-  roscpp
-  rospy
+  pluginlib
   sensor_msgs
   spinal
-  std_msgs
   tf
   tf_conversions
   jsk_recognition_msgs
@@ -33,7 +31,7 @@ generate_dynamic_reconfigure_options(
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES ${PROJECT_NAME} sensor_pluginlib
-  CATKIN_DEPENDS geodesy kalman_filter nodelet spinal tf tf_conversions
+  CATKIN_DEPENDS aerial_robot_model aerial_robot_msgs cv_bridge dynamic_reconfigure geodesy geographic_msgs geometry_msgs kalman_filter nav_msgs nodelet pluginlib sensor_msgs spinal tf tf_conversions jsk_recognition_msgs
 )
 
 include_directories(
@@ -43,7 +41,7 @@ include_directories(
 
 ### state estimation
 add_library(aerial_robot_estimation
-  src/state_estimation)
+  src/state_estimation.cpp)
 target_link_libraries(aerial_robot_estimation ${catkin_LIBRARIES})
 
 ### sensor plugins
@@ -66,7 +64,6 @@ target_link_libraries(kf_baro_bias_pluginlib ${catkin_LIBRARIES})
 add_dependencies(kf_baro_bias_pluginlib ${PROJECT_NAME}_gencfg)
 
 set(USE_GPU OFF)
-set(CMAKE_BUILD_TYPE Release)
 
 if(USE_GPU)
   add_definitions(-DUSE_GPU)
@@ -74,3 +71,15 @@ endif()
 
 add_library(optical_flow src/vision/optical_flow.cpp)
 target_link_libraries(optical_flow ${catkin_LIBRARIES})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS aerial_robot_estimation sensor_pluginlib kf_baro_bias_pluginlib optical_flow
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY launch plugins config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
diff --git a/aerial_robot_estimation/package.xml b/aerial_robot_estimation/package.xml
index ac895e825..85fd26b21 100644
--- a/aerial_robot_estimation/package.xml
+++ b/aerial_robot_estimation/package.xml
@@ -21,11 +21,8 @@
   <build_depend>nav_msgs</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>pluginlib</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>spinal</build_depend>
-  <build_depend>std_msgs</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>tf_conversions</build_depend>
   <build_depend>jsk_recognition_msgs</build_depend>
@@ -41,11 +38,8 @@
   <run_depend>nav_msgs</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>pluginlib</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>rospy</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>spinal</run_depend>
-  <run_depend>std_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>tf_conversions</run_depend>
diff --git a/aerial_robot_model/CMakeLists.txt b/aerial_robot_model/CMakeLists.txt
index 64dae79bd..d7003ca2c 100644
--- a/aerial_robot_model/CMakeLists.txt
+++ b/aerial_robot_model/CMakeLists.txt
@@ -1,6 +1,8 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot_model)
 
+add_compile_options(-std=c++14)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -12,7 +14,6 @@ find_package(catkin REQUIRED COMPONENTS
   kdl_parser
   message_generation
   pluginlib
-  roscpp
   sensor_msgs
   spinal
   std_msgs
@@ -32,9 +33,6 @@ find_package(urdfdom_headers REQUIRED)
 find_package(Eigen3 REQUIRED)
 
 include_directories(${orocos_kdl_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS})
-link_directories(${orocos_kdl_LIBRARY_DIRS})
-
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
 
 if(NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE RelWithDebInfo)
@@ -55,7 +53,8 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS include test
   LIBRARIES transformable_aerial_robot_model transformable_aerial_robot_model_ros numerical_jacobians
-  CATKIN_DEPENDS eigen_conversions interactive_markers spinal tf tf_conversions
+  CATKIN_DEPENDS eigen_conversions geometry_msgs interactive_markers kalman_filter kdl_parser message_runtime pluginlib sensor_msgs spinal std_msgs tf tf_conversions tf2_eigen tf2_geometry_msgs tf2_kdl tf2_ros tf_conversions urdf visualization_msgs
+  DEPENDS Eigen3 orocos_kdl urdfdom_headers
 )
 
 ###########
@@ -102,5 +101,18 @@ add_library(numerical_jacobians test/aerial_robot_model/numerical_jacobians.cpp)
 target_link_libraries(numerical_jacobians transformable_aerial_robot_model ${catkin_LIBRARIES})
 
 
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY include/${PROJECT_NAME}/ test/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS transformable_aerial_robot_model transformable_aerial_robot_model_ros servo_bridge numerical_jacobians
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(TARGETS rotor_tf_publisher interactive_marker_tf_broadcaster servo_bridge_node
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch script
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
diff --git a/aerial_robot_model/package.xml b/aerial_robot_model/package.xml
index 79dc72501..0909f02f4 100644
--- a/aerial_robot_model/package.xml
+++ b/aerial_robot_model/package.xml
@@ -19,7 +19,6 @@
   <build_depend>liburdfdom-headers-dev</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>pluginlib</build_depend>
-  <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>spinal</build_depend>
   <build_depend>std_msgs</build_depend>
@@ -42,7 +41,7 @@
   <run_depend>message_runtime</run_depend>
   <run_depend>pluginlib</run_depend>
   <run_depend>robot_state_publisher</run_depend>
-  <run_depend>roscpp</run_depend>
+  <run_depend>rviz</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>spinal</run_depend>
   <run_depend>std_msgs</run_depend>
diff --git a/aerial_robot_msgs/CMakeLists.txt b/aerial_robot_msgs/CMakeLists.txt
index 2e8f1d684..881eecdc3 100644
--- a/aerial_robot_msgs/CMakeLists.txt
+++ b/aerial_robot_msgs/CMakeLists.txt
@@ -1,8 +1,12 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot_msgs)
 
 ## Find catkin and any catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs message_generation)
+find_package(catkin REQUIRED COMPONENTS
+  std_msgs
+  geometry_msgs
+  message_generation
+  )
 
 ## Declare ROS messages and services
 add_message_files(FILES
@@ -19,11 +23,11 @@ add_message_files(FILES
 )
 
 ## Generate added messages and services
-generate_messages(DEPENDENCIES std_msgs std_msgs geometry_msgs)
+generate_messages(DEPENDENCIES std_msgs geometry_msgs)
 
 ## Declare a catkin package
 catkin_package(
-  CATKIN_DEPENDS message_runtime
+  CATKIN_DEPENDS std_msgs geometry_msgs message_runtime
 )
 
 
diff --git a/aerial_robot_msgs/package.xml b/aerial_robot_msgs/package.xml
index 1c439fb51..890dce372 100644
--- a/aerial_robot_msgs/package.xml
+++ b/aerial_robot_msgs/package.xml
@@ -11,14 +11,10 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>roscpp</build_depend>
-<!--  <build_depend>rospy</build_depend> -->
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>message_generation</build_depend>
 
-  <run_depend>roscpp</run_depend>
-<!--  <run_depend>rospy</run_depend> -->
   <run_depend>std_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>message_runtime</run_depend>
diff --git a/aerial_robot_nerve/motor_test/CMakeLists.txt b/aerial_robot_nerve/motor_test/CMakeLists.txt
index e559c37f5..573c29b66 100644
--- a/aerial_robot_nerve/motor_test/CMakeLists.txt
+++ b/aerial_robot_nerve/motor_test/CMakeLists.txt
@@ -1,165 +1,37 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(motor_test)
 
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+add_compile_options(-std=c++14)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
   std_srvs
   geometry_msgs
-  takasako_sps
 )
 
-## System dependencies are found with CMake's conventions
- find_package(Boost REQUIRED COMPONENTS system)
+find_package(Boost REQUIRED COMPONENTS system)
 
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependencies might have been
-##     pulled in transitively but can be declared for certainty nonetheless:
-##     * add a build_depend tag for "message_generation"
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  LIBRARIES force_sensor_log
-  CATKIN_DEPENDS roscpp std_msgs std_srvs
-)
+  CATKIN_DEPENDS roscpp std_msgs std_srvs geometry_msgs
 
-###########
-## Build ##
-###########
+)
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a cpp library
-# add_library(force_sensor_log
-#   src/${PROJECT_NAME}/force_sensor_log.cpp
-# )
-
-## Declare a cpp executable
- add_executable(motor_test_node src/motor_test.cpp)
+add_executable(motor_test_node src/motor_test.cpp)
 
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-# add_dependencies(force_sensor_log_node force_sensor_log_generate_messages_cpp)
-
-## Specify libraries to link a library or executable target against
- target_link_libraries(motor_test_node
+target_link_libraries(motor_test_node
    ${catkin_LIBRARIES}
  )
 
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
- install(TARGETS  motor_test_node
-   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
+install(TARGETS  motor_test_node
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
 
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_force_sensor_log.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+install(DIRECTORY scripts
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
diff --git a/aerial_robot_nerve/motor_test/package.xml b/aerial_robot_nerve/motor_test/package.xml
index 4646ebfff..17956070e 100644
--- a/aerial_robot_nerve/motor_test/package.xml
+++ b/aerial_robot_nerve/motor_test/package.xml
@@ -4,47 +4,15 @@
   <version>1.3.0</version>
   <description>The motor test package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="bakui@todo.todo">bakui</maintainer>
+  <maintainer email="chou@jsk.imi.i.u-tokyo.ac.jp">Moju Zhao</maintainer>
 
+  <license>BSD</license>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/force_sensor_log</url> -->
-
-
-  <!-- Author tags are optional, mutiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintianers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
-  <build_depend>takasako_sps</build_depend>
 
   <run_depend>geometry_msgs</run_depend>
   <run_depend>roscpp</run_depend>
@@ -52,12 +20,4 @@
   <run_depend>std_srvs</run_depend>
   <run_depend>takasako_sps</run_depend>
 
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- You can specify that this package is a metapackage here: -->
-    <!-- <metapackage/> -->
-
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
 </package>
diff --git a/aerial_robot_simulation/CMakeLists.txt b/aerial_robot_simulation/CMakeLists.txt
index ad69830d9..195b64f6c 100644
--- a/aerial_robot_simulation/CMakeLists.txt
+++ b/aerial_robot_simulation/CMakeLists.txt
@@ -1,48 +1,31 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(aerial_robot_simulation)
 
-## Add support for C++11, supported in ROS Kinetic and newer
-add_definitions(-std=c++11)
+add_compile_options(-std=c++11 ${GAZEBO_CXX_FLAGS})
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   aerial_robot_base
   aerial_robot_estimation
-  aerial_robot_model
   aerial_robot_msgs
+  aerial_robot_model
   gazebo_ros_control
+  kdl_parser
   roscpp
   spinal
-  kdl_parser
-)
+  tf
+  )
 
 # Depend on system install of Gazebo
 find_package(GAZEBO REQUIRED)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-
 find_package(orocos_kdl REQUIRED)
 find_package(urdfdom_headers REQUIRED)
 include_directories(${orocos_kdl_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS})
-link_directories(${orocos_kdl_LIBRARY_DIRS})
 
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-###################################
-## catkin specific configuration ##
-###################################
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES aerial_robot_hw_sim flight_controllers
-  CATKIN_DEPENDS gazebo_ros_control roscpp
-  DEPENDS orocos_kdl urdfdom_headers
+  LIBRARIES aerial_robot_hw_sim spinal_interface flight_controllers
+  CATKIN_DEPENDS aerial_robot_base aerial_robot_estimation aerial_robot_msgs aerial_robot_model gazebo_ros_control kdl_parser roscpp spinal tf
+  DEPENDS GAZEBO orocos_kdl urdfdom_headers
 )
 
 ###########
@@ -68,19 +51,19 @@ target_link_libraries(spinal_interface ${catkin_LIBRARIES})
 add_library(aerial_robot_hw_sim src/aerial_robot_hw_sim.cpp)
 target_link_libraries(aerial_robot_hw_sim spinal_interface ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
-#############
-## Install ##
-#############
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
 
-#############
-## Testing ##
-#############
+install(TARGETS aerial_robot_hw_sim spinal_interface flight_controllers
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
 
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_aerial_robot_simulation.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+install(DIRECTORY scripts
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
 
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+install(FILES aerial_robot_hw_sim_plugins.xml flight_controllers_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/aerial_robot_simulation/package.xml b/aerial_robot_simulation/package.xml
index 8bd043bcd..5bcde281a 100644
--- a/aerial_robot_simulation/package.xml
+++ b/aerial_robot_simulation/package.xml
@@ -8,7 +8,7 @@
 
   <license>BSD</license>
 
-  <author >Moju Zhao</author>
+  <author>Moju Zhao</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
@@ -20,9 +20,6 @@
   <build_depend>kdl_parser</build_depend>
   <build_depend>liburdfdom-headers-dev</build_depend>
   <build_depend>roscpp</build_depend>
-  <build_depend>ros_control</build_depend>
-  <build_depend>ros_control</build_depend>
-  <build_depend>ros_controllers</build_depend>
   <build_depend>spinal</build_depend>
   <build_depend>tf</build_depend>
 
@@ -30,12 +27,13 @@
   <run_depend>aerial_robot_estimation</run_depend>
   <run_depend>aerial_robot_msgs</run_depend>
   <run_depend>aerial_robot_model</run_depend>
+  <run_depend>controller_manager</run_depend>
   <run_depend>controller_interface</run_depend>
+  <run_depend>gazebo_ros</run_depend>
   <run_depend>gazebo_ros_control</run_depend>
   <run_depend>hector_gazebo_plugins</run_depend>
   <run_depend>kdl_parser</run_depend>
   <run_depend>roscpp</run_depend>
-  <run_depend>ros_control</run_depend>
   <run_depend>ros_controllers</run_depend>
   <run_depend>spinal</run_depend>
   <run_depend>tf</run_depend>
diff --git a/robots/dragon/CMakeLists.txt b/robots/dragon/CMakeLists.txt
index a2bdd0352..b4f14795c 100644
--- a/robots/dragon/CMakeLists.txt
+++ b/robots/dragon/CMakeLists.txt
@@ -1,14 +1,15 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(dragon)
 
 add_compile_options(-std=c++14)
 
 find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  hydrus
   aerial_robot_control
   aerial_robot_model
   aerial_robot_msgs
+  hydrus
+  pluginlib
+  roscpp
 )
 
 find_package(Eigen3 REQUIRED)
@@ -22,8 +23,8 @@ find_package(NLopt REQUIRED)
 catkin_package(
   INCLUDE_DIRS include test
   LIBRARIES dragon_robot_model dragon_aerial_robot_controllib dragon_navigation dragon_numerical_jacobians
-  CATKIN_DEPENDS hydrus
-  DEPENDS EIGEN3
+  CATKIN_DEPENDS   aerial_robot_control aerial_robot_model aerial_robot_msgs hydrus pluginlib roscpp
+  DEPENDS EIGEN3 NLopt
 )
 
 ###########
@@ -61,7 +62,30 @@ target_link_libraries(dragon_numerical_jacobians dragon_robot_model ${catkin_LIB
 add_executable(dragon_jacobian_test test/dragon/jacobian_test.cpp)
 target_link_libraries(dragon_jacobian_test dragon_numerical_jacobians ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
 
+install(DIRECTORY include/${PROJECT_NAME}/ test/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS dragon_jacobian_test
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS dragon_sensor_pluginlib dragon_robot_model dragon_aerial_robot_controllib dragon_navigation dragon_numerical_jacobians
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY config scripts plugins launch images
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+install(DIRECTORY bin
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS rostest)
+  find_package(rostest REQUIRED)
   add_subdirectory(test)
 endif()
+
diff --git a/robots/dragon/package.xml b/robots/dragon/package.xml
index 9ddd28fec..38020bed4 100644
--- a/robots/dragon/package.xml
+++ b/robots/dragon/package.xml
@@ -16,17 +16,19 @@
   <build_depend>aerial_robot_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>hydrus</build_depend>
-  <build_depend>cmake_modules</build_depend>
   <build_depend>pluginlib</build_depend>
 
   <run_depend>aerial_robot_control</run_depend>
   <run_depend>aerial_robot_model</run_depend>
   <run_depend>aerial_robot_msgs</run_depend>
   <run_depend>aerial_robot_estimation</run_depend>
+  <run_depend>aerial_robot_simulation</run_depend>
+  <run_depend>aerial_robot_base</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>hydrus</run_depend>
   <run_depend>pluginlib</run_depend>
-  <run_depend>rostest</run_depend>
+
+  <test_depend>rostest</test_depend>
 
   <export>
     <aerial_robot_model plugin="${prefix}/plugins/robot_model_plugins.xml"/>
diff --git a/robots/hydrus/CMakeLists.txt b/robots/hydrus/CMakeLists.txt
index d869241f4..75d5713c1 100644
--- a/robots/hydrus/CMakeLists.txt
+++ b/robots/hydrus/CMakeLists.txt
@@ -1,9 +1,8 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(hydrus)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
+add_compile_options(-std=c++14)
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   aerial_robot_msgs
@@ -18,16 +17,14 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Eigen3 REQUIRED)
 
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-
 generate_dynamic_reconfigure_options(
   cfg/LQI.cfg)
 
 catkin_package(
   INCLUDE_DIRS include test
   LIBRARIES hydrus_robot_model hydrus_controller_pluginlib hydrus_numerical_jacobians
-  CATKIN_DEPENDS roscpp aerial_robot_control aerial_robot_model aerial_robot_msgs spinal tf_conversions eigen_conversions
-  DEPENDS EIGEN3
+  CATKIN_DEPENDS roscpp aerial_robot_msgs aerial_robot_control aerial_robot_model spinal eigen_conversions tf_conversions std_srvs dynamic_reconfigure
+  DEPENDS EIGEN3 
 )
 
 # Eigen requires optimization to get good performance
@@ -72,8 +69,31 @@ target_link_libraries(hydrus_numerical_jacobians hydrus_robot_model ${catkin_LIB
 add_executable(hydrus_jacobian_test test/hydrus/jacobian_test.cpp)
 target_link_libraries(hydrus_jacobian_test hydrus_numerical_jacobians ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
 
+
+install(DIRECTORY include/${PROJECT_NAME}/ test/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS hydrus_jacobian_test
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS hydrus_robot_model hydrus_controller_pluginlib hydrus_numerical_jacobians
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY config scripts plugins launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+install(DIRECTORY bin
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS rostest)
+  find_package(rostest REQUIRED)
   add_subdirectory(test)
 endif()
 
diff --git a/robots/hydrus/package.xml b/robots/hydrus/package.xml
index 19ea02799..823a11b38 100644
--- a/robots/hydrus/package.xml
+++ b/robots/hydrus/package.xml
@@ -14,7 +14,6 @@
   <build_depend>aerial_robot_control</build_depend>
   <build_depend>aerial_robot_model</build_depend>
   <build_depend>aerial_robot_msgs</build_depend>
-  <build_depend>cmake_modules</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>eigen_conversions</build_depend>
   <build_depend>roscpp</build_depend>
@@ -22,17 +21,20 @@
   <build_depend>std_srvs</build_depend>
   <build_depend>tf_conversions</build_depend>
 
+  <run_depend>aerial_robot_base</run_depend>
   <run_depend>aerial_robot_control</run_depend>
   <run_depend>aerial_robot_model</run_depend>
   <run_depend>aerial_robot_msgs</run_depend>
+  <run_depend>aerial_robot_simulation</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>eigen_conversions</run_depend>
   <run_depend>roscpp</run_depend>
-  <run_depend>rostest</run_depend>
   <run_depend>spinal</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>tf_conversions</run_depend>
 
+  <test_depend>rostest</test_depend>
+
   <export>
     <aerial_robot_control plugin="${prefix}/plugins/flight_control_plugins.xml" />
     <aerial_robot_model plugin="${prefix}/plugins/robot_model_plugins.xml"/>
diff --git a/robots/hydrus_xi/CMakeLists.txt b/robots/hydrus_xi/CMakeLists.txt
index 1856564d0..3f883c55c 100644
--- a/robots/hydrus_xi/CMakeLists.txt
+++ b/robots/hydrus_xi/CMakeLists.txt
@@ -1,12 +1,14 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(hydrus_xi)
 
 add_compile_options(-std=c++14)
 
 find_package(catkin REQUIRED COMPONENTS
+  aerial_robot_base
   aerial_robot_control
   aerial_robot_model
   aerial_robot_msgs
+  aerial_robot_simulation
   angles
   hydrus
   roscpp
@@ -14,15 +16,14 @@ find_package(catkin REQUIRED COMPONENTS
   spinal
   )
 
-find_package(rostest REQUIRED)
-
 find_package(NLopt REQUIRED)
 find_package(OsqpEigen REQUIRED)
 
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES  hydrus_xi_fully_actuated_robot_model
-  CATKIN_DEPENDS aerial_robot_control aerial_robot_model aerial_robot_msgs roscpp rospy spinal
+  CATKIN_DEPENDS  aerial_robot_base aerial_robot_control aerial_robot_model aerial_robot_msgs aerial_robot_simulation angles hydrus roscpp rospy spinal
+  DEPENDS NLopt OsqpEigen
   )
 
 if(NOT CMAKE_BUILD_TYPE)
@@ -48,7 +49,29 @@ target_link_libraries(hydrus_xi_fully_actuated_robot_model ${catkin_LIBRARIES})
 add_executable(hydrus_xi_jacobian_test test/hydrus_xi/jacobian_test.cpp)
 target_link_libraries(hydrus_xi_jacobian_test hydrus_xi_fully_actuated_robot_model ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
 
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS hydrus_xi_jacobian_test
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS hydrus_xi_under_actuated_navigation hydrus_xi_fully_actuated_robot_model
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(DIRECTORY config scripts plugins launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+install(FILES flight_navigation_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS rostest)
+  find_package(rostest REQUIRED)
   add_subdirectory(test)
 endif()
diff --git a/robots/hydrus_xi/package.xml b/robots/hydrus_xi/package.xml
index f06fd020f..d6f6334e7 100644
--- a/robots/hydrus_xi/package.xml
+++ b/robots/hydrus_xi/package.xml
@@ -9,16 +9,19 @@
   <license>B.S.D</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>aerial_robot_base</depend>
   <depend>aerial_robot_control</depend>
   <depend>aerial_robot_model</depend>
   <depend>aerial_robot_msgs</depend>
+  <depend>aerial_robot_simulation</depend>
   <depend>angles</depend>
   <depend>hydrus</depend>
   <depend>roscpp</depend>
   <depend>rospy</depend>
-  <depend>rostest</depend>
   <depend>spinal</depend>
 
+  <test_depend>rostest</test_depend>
+
   <export>
     <aerial_robot_model plugin="${prefix}/plugins/robot_model_plugin.xml"/>
     <aerial_robot_control plugin="${prefix}/flight_navigation_plugin.xml" />