diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..7af7c0f54 --- /dev/null +++ b/.clang-format @@ -0,0 +1,63 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: true +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: + AfterClass: true + AfterControlStatement: 'true' + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false diff --git a/.github/workflows/uml.yml b/.github/workflows/uml.yml new file mode 100644 index 000000000..72f11d9f5 --- /dev/null +++ b/.github/workflows/uml.yml @@ -0,0 +1,19 @@ +name: generate plantuml +on: push +jobs: + generate_plantuml: + runs-on: ubuntu-latest + name: plantuml + steps: + - name: checkout + uses: actions/checkout@v1 + with: + fetch-depth: 1 + - name: plantuml + id: plantuml + uses: grassedge/generate-plantuml-action@v1.5 + with: + path: docs/UMLs + message: "Render PlantUML files" + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file diff --git a/.gitignore b/.gitignore index 68802e703..3aa6090b3 100644 --- a/.gitignore +++ b/.gitignore @@ -6,7 +6,8 @@ .LSOverride # Icon must end with two \r -Icon +Icon + # Thumbnails ._* @@ -101,4 +102,8 @@ tags # python -*.pyc \ No newline at end of file +*.pyc + +# Jetbrains +.idea/ +*cmake-build-debug/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..5dc508125 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,32 @@ +--- +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v2.3.0 + hooks: + - id: check-yaml + - id: check-xml + - id: check-json + - id: end-of-file-fixer + - id: trailing-whitespace + - id: check-added-large-files + + # Python + - repo: https://github.com/psf/black + rev: 22.10.0 + hooks: + - id: black + language: python + args: [--line-length=120] + + # C++ + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v17.0.3 + hooks: + - id: clang-format + +# # YAML - still bad support for comments in YAML files +# - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt +# rev: 0.2.1 # or other specific tag +# hooks: +# - id: yamlfmt +# args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '120'] diff --git a/README.md b/README.md index f2f8df9de..7a38081be 100644 --- a/README.md +++ b/README.md @@ -22,5 +22,3 @@ catkin build ## Demo Please check instruction in [wiki](https://github.com/JSKAerialRobot/aerial_robot/wiki). - - diff --git a/aerial_robot_base/config/sensors/altimeter/baro.yaml b/aerial_robot_base/config/sensors/altimeter/baro.yaml index aa1fd194a..f91582931 100644 --- a/aerial_robot_base/config/sensors/altimeter/baro.yaml +++ b/aerial_robot_base/config/sensors/altimeter/baro.yaml @@ -1,8 +1,8 @@ sensor_plugin: - altitude: - barometer_sub_name: baro - baro_noise_sigma: 0.2 - baro_bias_noise_sigma: 0.001 - rx_freq: 80.0 - cutoff_freq: 10.0 - high_cutoff_freq: 1.0 + altitude: + barometer_sub_name: baro + baro_noise_sigma: 0.2 + baro_bias_noise_sigma: 0.001 + rx_freq: 80.0 + cutoff_freq: 10.0 + high_cutoff_freq: 1.0 diff --git a/aerial_robot_base/config/sensors/altimeter/leddar_one.yaml b/aerial_robot_base/config/sensors/altimeter/leddar_one.yaml index 76464b45d..80ef54209 100644 --- a/aerial_robot_base/config/sensors/altimeter/leddar_one.yaml +++ b/aerial_robot_base/config/sensors/altimeter/leddar_one.yaml @@ -1,9 +1,9 @@ sensor_plugin: - alt: - range_noise_sigma: 0.01 #old: 0.005 - range_sensor_sub_name: leddar_one/range - sensor_frame: leddarone - reference_frame: fc - no_height_offset: true - time_sync: false - delay: 0 + alt: + range_noise_sigma: 0.01 #old: 0.005 + range_sensor_sub_name: leddar_one/range + sensor_frame: leddarone + reference_frame: fc + no_height_offset: true + time_sync: false + delay: 0 diff --git a/aerial_robot_base/config/sensors/altimeter/sonar.yaml b/aerial_robot_base/config/sensors/altimeter/sonar.yaml index 39eb82e4c..772b7e88e 100644 --- a/aerial_robot_base/config/sensors/altimeter/sonar.yaml +++ b/aerial_robot_base/config/sensors/altimeter/sonar.yaml @@ -1,7 +1,6 @@ sensor_plugin: - range_sensor: - range_noise_sigma: 0.02 - range_sensor_sub_name: /sonar - ascending_check_range: 0.095 - calibrate_cnt: 10 - \ No newline at end of file + range_sensor: + range_noise_sigma: 0.02 + range_sensor_sub_name: /sonar + ascending_check_range: 0.095 + calibrate_cnt: 10 diff --git a/aerial_robot_base/config/sensors/gps/ublox_m8n.yaml b/aerial_robot_base/config/sensors/gps/ublox_m8n.yaml index c041d2a9f..dabee52a1 100644 --- a/aerial_robot_base/config/sensors/gps/ublox_m8n.yaml +++ b/aerial_robot_base/config/sensors/gps/ublox_m8n.yaml @@ -1,11 +1,11 @@ sensor_plugin: - gps: #single GPS - min_est_sat_num: 7 - vel_noise_sigma: 0.02 #0.05 -> 0.01 -> 0.02 - pos_noise_sigma: 1.0 - gps_ros_sub_name: sim/gps/fix - sensor_frame: gps - ned_flag: true #north(x)-east(y)-down(z) frame - time_sync: true - delay: -0.45 # daylight: -0.45, night: -0.5 - #only_use_vel: true # debug + gps: #single GPS + min_est_sat_num: 7 + vel_noise_sigma: 0.02 #0.05 -> 0.01 -> 0.02 + pos_noise_sigma: 1.0 + gps_ros_sub_name: sim/gps/fix + sensor_frame: gps + ned_flag: true #north(x)-east(y)-down(z) frame + time_sync: true + delay: -0.45 # daylight: -0.45, night: -0.5 + #only_use_vel: true # debug diff --git a/aerial_robot_base/config/sensors/gps/ublox_m9n.yaml b/aerial_robot_base/config/sensors/gps/ublox_m9n.yaml index 683bb7d0e..e8bedcd9b 100644 --- a/aerial_robot_base/config/sensors/gps/ublox_m9n.yaml +++ b/aerial_robot_base/config/sensors/gps/ublox_m9n.yaml @@ -1,11 +1,11 @@ sensor_plugin: - gps: #single GPS - min_est_sat_num: 10 - vel_noise_sigma: 0.02 #0.05 -> 0.01 -> 0.02 - pos_noise_sigma: 1.0 - gps_ros_sub_name: sim/gps/fix - sensor_frame: gps - ned_flag: true #north(x)-east(y)-down(z) frame - time_sync: true - delay: -0.35 # alternative: -0.25 # Ublox M9N normal: -0.25 ~ 0.35, -0.35 is good @2021.05.24 Kashiwa. TODO: auto-calibra with VIO - #only_use_vel: true # debug + gps: #single GPS + min_est_sat_num: 10 + vel_noise_sigma: 0.02 #0.05 -> 0.01 -> 0.02 + pos_noise_sigma: 1.0 + gps_ros_sub_name: sim/gps/fix + sensor_frame: gps + ned_flag: true #north(x)-east(y)-down(z) frame + time_sync: true + delay: -0.35 # alternative: -0.25 # Ublox M9N normal: -0.25 ~ 0.35, -0.35 is good @2021.05.24 Kashiwa. TODO: auto-calibra with VIO + #only_use_vel: true # debug diff --git a/aerial_robot_base/config/sensors/gps/ublox_rtk_m8p.yaml b/aerial_robot_base/config/sensors/gps/ublox_rtk_m8p.yaml index 785ce4bef..041cb09e4 100644 --- a/aerial_robot_base/config/sensors/gps/ublox_rtk_m8p.yaml +++ b/aerial_robot_base/config/sensors/gps/ublox_rtk_m8p.yaml @@ -2,15 +2,14 @@ # Note: we need a normal GPS as gps1, otherwise following yaml file would be never loaded sensor_plugin: - gps2: # we need a normal GPS as gps1, otherwise following yaml file would be never loaded - rtk: true - min_est_sat_num: 10 - pos_noise_sigma: 1.0 - gps_ros_sub_name: rtk_gps/fix - rtk_pos_sub_name: rtk_gps/rel_pos - sensor_frame: rtk_gps - ned_flag: true #north(x)-east(y)-down(z) frame - time_sync: true - delay: -0.25 - #rtk_offset: false # Please overwrite this parameter in your StateEstimation.yaml. **Not Here!!*. True: set the takeoff point as zero for RTK model, otherwise, the zero point is the base station like mocap. Default is false - + gps2: # we need a normal GPS as gps1, otherwise following yaml file would be never loaded + rtk: true + min_est_sat_num: 10 + pos_noise_sigma: 1.0 + gps_ros_sub_name: rtk_gps/fix + rtk_pos_sub_name: rtk_gps/rel_pos + sensor_frame: rtk_gps + ned_flag: true #north(x)-east(y)-down(z) frame + time_sync: true + delay: -0.25 + #rtk_offset: false # Please overwrite this parameter in your StateEstimation.yaml. **Not Here!!*. True: set the takeoff point as zero for RTK model, otherwise, the zero point is the base station like mocap. Default is false diff --git a/aerial_robot_base/config/sensors/imu/spinal.yaml b/aerial_robot_base/config/sensors/imu/spinal.yaml index a3a92f89f..2732b25f1 100644 --- a/aerial_robot_base/config/sensors/imu/spinal.yaml +++ b/aerial_robot_base/config/sensors/imu/spinal.yaml @@ -1,9 +1,9 @@ sensor_plugin: - imu: - imu_topic_name: imu - level_acc_noise_sigma: 0.05 - z_acc_noise_sigma: 0.05 - level_acc_bias_noise_sigma: 0.001 # (> 0 means to do the acc bias estimation by kalman filter) - z_acc_bias_noise_sigma: 0.001 # (= 0 means not to do the acc bias estimation by kalman filter) - angle_bias_noise_sigma: 0.0002 #good for EKF - calib_time: 2.0 + imu: + imu_topic_name: imu + level_acc_noise_sigma: 0.05 + z_acc_noise_sigma: 0.05 + level_acc_bias_noise_sigma: 0.001 # (> 0 means to do the acc bias estimation by kalman filter) + z_acc_bias_noise_sigma: 0.001 # (= 0 means not to do the acc bias estimation by kalman filter) + angle_bias_noise_sigma: 0.0002 #good for EKF + calib_time: 2.0 diff --git a/aerial_robot_base/config/sensors/mocap.yaml b/aerial_robot_base/config/sensors/mocap.yaml index 9e1b1b321..aa8e5d832 100644 --- a/aerial_robot_base/config/sensors/mocap.yaml +++ b/aerial_robot_base/config/sensors/mocap.yaml @@ -1,9 +1,9 @@ sensor_plugin: - mocap: - pos_noise_sigma: 0.0002 - angle_sigma: 0.005 - acc_bias_noise_sigma: 0.001 # force acc bias estimation by kalman filter - unhealth_level: 3 - health_timeout: 0.5 - time_sync: false - mocap_sub_name: mocap/pose + mocap: + pos_noise_sigma: 0.0002 + angle_sigma: 0.005 + acc_bias_noise_sigma: 0.001 # force acc bias estimation by kalman filter + unhealth_level: 3 + health_timeout: 0.5 + time_sync: false + mocap_sub_name: mocap/pose diff --git a/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h b/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h index df6ffd1a3..270696736 100644 --- a/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h +++ b/aerial_robot_base/include/aerial_robot_base/aerial_robot_base.h @@ -12,19 +12,19 @@ using namespace std; class AerialRobotBase { - public: - AerialRobotBase(ros::NodeHandle nh, ros::NodeHandle nh_private); +public: + AerialRobotBase(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); ~AerialRobotBase(); - void mainFunc(const ros::TimerEvent & e); + void mainFunc(const ros::TimerEvent& e); - private: +private: ros::NodeHandle nh_; ros::NodeHandle nhp_; ros::Timer main_timer_; boost::shared_ptr robot_model_ros_; - boost::shared_ptr estimator_; + boost::shared_ptr estimator_; pluginlib::ClassLoader navigator_loader_; boost::shared_ptr navigator_; @@ -32,7 +32,7 @@ class AerialRobotBase pluginlib::ClassLoader controller_loader_; boost::shared_ptr controller_; - ros::AsyncSpinner callback_spinner_; // Use 4 threads - ros::AsyncSpinner main_loop_spinner_; // Use 1 threads + ros::AsyncSpinner callback_spinner_; // Use 4 threads + ros::AsyncSpinner main_loop_spinner_; // Use 1 threads ros::CallbackQueue main_loop_queue_; }; diff --git a/aerial_robot_base/launch/external_module/joy_stick.launch b/aerial_robot_base/launch/external_module/joy_stick.launch index ef603a102..c4da2be8f 100644 --- a/aerial_robot_base/launch/external_module/joy_stick.launch +++ b/aerial_robot_base/launch/external_module/joy_stick.launch @@ -1,11 +1,11 @@ - + - + - - - + + + diff --git a/aerial_robot_base/launch/external_module/mocap.launch b/aerial_robot_base/launch/external_module/mocap.launch index 8a083dac1..f94f5dfa2 100644 --- a/aerial_robot_base/launch/external_module/mocap.launch +++ b/aerial_robot_base/launch/external_module/mocap.launch @@ -1,26 +1,26 @@ - + - - - rigid_bodies: - '$(arg robot_id)': - pose: mocap/pose - pose2d: mocap/ground_pose - child_frame_id: baselink - parent_frame_id: world - optitrack_config: - multicast_address: 239.255.42.99 - command_port: 1510 - data_port: 1511 - - - + + + rigid_bodies: + '$(arg robot_id)': + pose: mocap/pose + pose2d: mocap/ground_pose + child_frame_id: baselink + parent_frame_id: world + optitrack_config: + multicast_address: 239.255.42.99 + command_port: 1510 + data_port: 1511 + + + diff --git a/aerial_robot_base/launch/external_module/rtk_gps.launch b/aerial_robot_base/launch/external_module/rtk_gps.launch index 5a84d9354..732da7d06 100644 --- a/aerial_robot_base/launch/external_module/rtk_gps.launch +++ b/aerial_robot_base/launch/external_module/rtk_gps.launch @@ -1,19 +1,19 @@ - + - - + + - - - - - + + + + + - - - - + + + + diff --git a/aerial_robot_base/scripts/keyboard_command.py b/aerial_robot_base/scripts/keyboard_command.py index be8ce6016..a187ffe9a 100755 --- a/aerial_robot_base/scripts/keyboard_command.py +++ b/aerial_robot_base/scripts/keyboard_command.py @@ -9,78 +9,77 @@ import sys, select, termios, tty -def getKey(): - tty.setraw(sys.stdin.fileno()) - select.select([sys.stdin], [], [], 0) - key = sys.stdin.read(1) - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) - return key -if __name__=="__main__": - settings = termios.tcgetattr(sys.stdin) - rospy.init_node("keyboard_command") - robot_ns = rospy.get_param("~robot_ns", ""); - - if not robot_ns: - master = rosgraph.Master('/rostopic') - try: - _, subs, _ = master.getSystemState() +def getKey(): + tty.setraw(sys.stdin.fileno()) + select.select([sys.stdin], [], [], 0) + key = sys.stdin.read(1) + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key - except socket.error: - raise ROSTopicIOException("Unable to communicate with master!") - teleop_topics = [topic[0] for topic in subs if 'teleop_command/start' in topic[0]] - if len(teleop_topics) == 1: - robot_ns = teleop_topics[0].split('/teleop')[0] +if __name__ == "__main__": + settings = termios.tcgetattr(sys.stdin) + rospy.init_node("keyboard_command") + robot_ns = rospy.get_param("~robot_ns", "") - ns = robot_ns + "/teleop_command" - land_pub = rospy.Publisher(ns + '/land', Empty, queue_size=1) - halt_pub = rospy.Publisher(ns + '/halt', Empty, queue_size=1) - start_pub = rospy.Publisher(ns + '/start', Empty, queue_size=1) - takeoff_pub = rospy.Publisher(ns + '/takeoff', Empty, queue_size=1) - force_landing_pub = rospy.Publisher(ns + '/force_landing', Empty, queue_size=1) - ctrl_mode_pub = rospy.Publisher(ns + '/ctrl_mode', Int8, queue_size=1) - motion_start_pub = rospy.Publisher('task_start', Empty, queue_size=1) + if not robot_ns: + master = rosgraph.Master("/rostopic") + try: + _, subs, _ = master.getSystemState() + except socket.error: + raise ROSTopicIOException("Unable to communicate with master!") - #the way to write publisher in python - comm=Int8() - gain=UInt16() - try: - while(True): - key = getKey() - print("the key value is {}".format(ord(key))) - # takeoff and landing - if key == 'l': - land_pub.publish(Empty()) - #for hydra joints - if key == 'r': - start_pub.publish(Empty()) - #for hydra joints - if key == 'h': - halt_pub.publish(Empty()) - #for hydra joints - if key == 'f': - force_landing_pub.publish(Empty()) - if key == 't': - takeoff_pub.publish(Empty()) - if key == 'u': - stair_pub.publish(Empty()) - if key == 'x': - motion_start_pub.publish() - if key == 'v': - comm.data = 1 - ctrl_mode_pub.publish(comm) - if key == 'p': - comm.data = 0 - ctrl_mode_pub.publish(comm) - if key == '\x03': - break - rospy.sleep(0.001) + teleop_topics = [topic[0] for topic in subs if "teleop_command/start" in topic[0]] + if len(teleop_topics) == 1: + robot_ns = teleop_topics[0].split("/teleop")[0] - except Exception as e: - print(repr(e)) - finally: - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + ns = robot_ns + "/teleop_command" + land_pub = rospy.Publisher(ns + "/land", Empty, queue_size=1) + halt_pub = rospy.Publisher(ns + "/halt", Empty, queue_size=1) + start_pub = rospy.Publisher(ns + "/start", Empty, queue_size=1) + takeoff_pub = rospy.Publisher(ns + "/takeoff", Empty, queue_size=1) + force_landing_pub = rospy.Publisher(ns + "/force_landing", Empty, queue_size=1) + ctrl_mode_pub = rospy.Publisher(ns + "/ctrl_mode", Int8, queue_size=1) + motion_start_pub = rospy.Publisher("task_start", Empty, queue_size=1) + # the way to write publisher in python + comm = Int8() + gain = UInt16() + try: + while True: + key = getKey() + print("the key value is {}".format(ord(key))) + # takeoff and landing + if key == "l": + land_pub.publish(Empty()) + # for hydra joints + if key == "r": + start_pub.publish(Empty()) + # for hydra joints + if key == "h": + halt_pub.publish(Empty()) + # for hydra joints + if key == "f": + force_landing_pub.publish(Empty()) + if key == "t": + takeoff_pub.publish(Empty()) + if key == "u": + stair_pub.publish(Empty()) + if key == "x": + motion_start_pub.publish() + if key == "v": + comm.data = 1 + ctrl_mode_pub.publish(comm) + if key == "p": + comm.data = 0 + ctrl_mode_pub.publish(comm) + if key == "\x03": + break + rospy.sleep(0.001) + except Exception as e: + print(repr(e)) + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) diff --git a/aerial_robot_base/scripts/rms.py b/aerial_robot_base/scripts/rms.py index 2f87894a6..ce6286e6b 100755 --- a/aerial_robot_base/scripts/rms.py +++ b/aerial_robot_base/scripts/rms.py @@ -14,76 +14,85 @@ h: stop the calculate the output the RMS results. """ + def cb(data): - global start_flag - if start_flag == True: - global pose_cnt - global pose_squared_errors_sum - pose_cnt = pose_cnt + 1 + global start_flag + if start_flag == True: + global pose_cnt + global pose_squared_errors_sum + pose_cnt = pose_cnt + 1 - pose_squared_errors_sum[0] = pose_squared_errors_sum[0] + data.x.err_p * data.x.err_p - pose_squared_errors_sum[1] = pose_squared_errors_sum[1] + data.y.err_p * data.y.err_p - pose_squared_errors_sum[2] = pose_squared_errors_sum[2] + data.z.err_p * data.z.err_p + pose_squared_errors_sum[0] = pose_squared_errors_sum[0] + data.x.err_p * data.x.err_p + pose_squared_errors_sum[1] = pose_squared_errors_sum[1] + data.y.err_p * data.y.err_p + pose_squared_errors_sum[2] = pose_squared_errors_sum[2] + data.z.err_p * data.z.err_p - pose_squared_errors_sum[3] = pose_squared_errors_sum[3] + data.roll.err_p * data.roll.err_p - pose_squared_errors_sum[4] = pose_squared_errors_sum[4] + data.pitch.err_p * data.pitch.err_p - pose_squared_errors_sum[5] = pose_squared_errors_sum[5] + data.yaw.err_p * data.yaw.err_p + pose_squared_errors_sum[3] = pose_squared_errors_sum[3] + data.roll.err_p * data.roll.err_p + pose_squared_errors_sum[4] = pose_squared_errors_sum[4] + data.pitch.err_p * data.pitch.err_p + pose_squared_errors_sum[5] = pose_squared_errors_sum[5] + data.yaw.err_p * data.yaw.err_p - rms = [math.sqrt(i / pose_cnt) for i in pose_squared_errors_sum] - #rospy.loginfo("RMS errors of pos: [%f, %f, %f]; rot: [%f, %f, %f]", rms[0], rms[1], rms[2], rms[3], rms[4], rms[5]) + rms = [math.sqrt(i / pose_cnt) for i in pose_squared_errors_sum] + # rospy.loginfo("RMS errors of pos: [%f, %f, %f]; rot: [%f, %f, %f]", rms[0], rms[1], rms[2], rms[3], rms[4], rms[5]) -def getKey(): - tty.setraw(sys.stdin.fileno()) - select.select([sys.stdin], [], [], 0) - key = sys.stdin.read(1) - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) - return key -if __name__=="__main__": - settings = termios.tcgetattr(sys.stdin) +def getKey(): + tty.setraw(sys.stdin.fileno()) + select.select([sys.stdin], [], [], 0) + key = sys.stdin.read(1) + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key - start_flag = False - msg_cnt = 0 - pose_squared_errors_sum = [0] * 6 - pose_cnt = 0 +if __name__ == "__main__": + settings = termios.tcgetattr(sys.stdin) - rospy.Subscriber("debug/pose/pid", PoseControlPid, cb) + start_flag = False + msg_cnt = 0 - rospy.init_node('rms_error') + pose_squared_errors_sum = [0] * 6 + pose_cnt = 0 - print(msg) - try: - while(True): - key = getKey() + rospy.Subscriber("debug/pose/pid", PoseControlPid, cb) - if key == 's': - rospy.loginfo("start to calculate RMS errors") - start_flag = True - if key == 'h': - rospy.loginfo("stop calculation") + rospy.init_node("rms_error") - rms = [0] * 6 - if pose_cnt > 0: - rms = [math.sqrt(i / pose_cnt) for i in pose_squared_errors_sum] + print(msg) + try: + while True: + key = getKey() - rospy.loginfo("RMS of pos errors: [%f, %f, %f], att errors: [%f, %f, %f]", rms[0], rms[1], rms[2], rms[3], rms[4], rms[5]) + if key == "s": + rospy.loginfo("start to calculate RMS errors") + start_flag = True + if key == "h": + rospy.loginfo("stop calculation") - start_flag = False + rms = [0] * 6 + if pose_cnt > 0: + rms = [math.sqrt(i / pose_cnt) for i in pose_squared_errors_sum] - pose_cnt = 0 - pose_squared_errors_sum = [0] * 6 + rospy.loginfo( + "RMS of pos errors: [%f, %f, %f], att errors: [%f, %f, %f]", + rms[0], + rms[1], + rms[2], + rms[3], + rms[4], + rms[5], + ) - else: - if (key == '\x03'): - break - rospy.sleep(0.001) + start_flag = False - except Exception as e: - print(e) - print(repr(e)) + pose_cnt = 0 + pose_squared_errors_sum = [0] * 6 - finally: - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + else: + if key == "\x03": + break + rospy.sleep(0.001) + except Exception as e: + print(e) + print(repr(e)) + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) diff --git a/aerial_robot_base/src/aerial_robot_base.cpp b/aerial_robot_base/src/aerial_robot_base.cpp index f613b4772..23661e858 100644 --- a/aerial_robot_base/src/aerial_robot_base.cpp +++ b/aerial_robot_base/src/aerial_robot_base.cpp @@ -1,16 +1,18 @@ #include -AerialRobotBase::AerialRobotBase(ros::NodeHandle nh, ros::NodeHandle nh_private) - : nh_(nh), nhp_(nh_private), callback_spinner_(4), main_loop_spinner_(1, &main_loop_queue_), - controller_loader_("aerial_robot_control", "aerial_robot_control::ControlBase"), - navigator_loader_("aerial_robot_control", "aerial_robot_navigation::BaseNavigator") +AerialRobotBase::AerialRobotBase(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) + : nh_(nh) + , nhp_(nh_private) + , callback_spinner_(4) + , main_loop_spinner_(1, &main_loop_queue_) + , controller_loader_("aerial_robot_control", "aerial_robot_control::ControlBase") + , navigator_loader_("aerial_robot_control", "aerial_robot_navigation::BaseNavigator") { - bool param_verbose; - nhp_.param ("param_verbose", param_verbose, true); + nhp_.param("param_verbose", param_verbose, true); double main_rate; - nhp_.param ("main_rate", main_rate, 0.0); + nhp_.param("main_rate", main_rate, 0.0); // robot model robot_model_ros_ = boost::make_shared(nh_, nhp_); @@ -22,70 +24,70 @@ AerialRobotBase::AerialRobotBase(ros::NodeHandle nh, ros::NodeHandle nh_private) // navigation std::string navi_plugin_name; - if(nh_.getParam("flight_navigation_plugin_name", navi_plugin_name)) + if (nh_.getParam("flight_navigation_plugin_name", navi_plugin_name)) + { + try { - try - { - navigator_ = navigator_loader_.createInstance(navi_plugin_name); - } - catch(pluginlib::PluginlibException& ex) - { - ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); - } + navigator_ = navigator_loader_.createInstance(navi_plugin_name); } - else + catch (pluginlib::PluginlibException& ex) { - ROS_DEBUG("use default class for flight navigation: aerial_robot_navigation::BaseNavigator"); - navigator_ = boost::make_shared(); + ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); } + } + else + { + ROS_DEBUG("use default class for flight navigation: aerial_robot_navigation::BaseNavigator"); + navigator_ = boost::make_shared(); + } navigator_->initialize(nh_, nhp_, robot_model, estimator_); // controller try - { - std::string aerial_robot_control_name; - nh_.param ("aerial_robot_control_name", aerial_robot_control_name, std::string("aerial_robot_control/flatness_pid")); - controller_ = controller_loader_.createInstance(aerial_robot_control_name); - controller_->initialize(nh_, nhp_, robot_model, estimator_, navigator_, main_rate); - } - catch(pluginlib::PluginlibException& ex) - { - ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); - } + { + std::string aerial_robot_control_name; + nh_.param("aerial_robot_control_name", aerial_robot_control_name, std::string("aerial_robot_control/flatness_pid")); + controller_ = controller_loader_.createInstance(aerial_robot_control_name); + controller_->initialize(nh_, nhp_, robot_model, estimator_, navigator_, main_rate); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); + } - if(param_verbose) cout << nhp_.getNamespace() << ": main_rate is " << main_rate << endl; - if(main_rate <= 0) + if (param_verbose) + cout << nhp_.getNamespace() << ": main_rate is " << main_rate << endl; + if (main_rate <= 0) ROS_ERROR_STREAM("mian rate is negative, can not run the main timer"); else - { - // note1: separate the thread for main control (including navigation) loop to guarantee a relatively stable loop rate - - ros::TimerOptions ops(ros::Duration(1.0 / main_rate), - boost::bind(&AerialRobotBase::mainFunc, this, _1), - &main_loop_queue_); - main_timer_ = nhp_.createTimer(ops); - main_loop_spinner_.start(); - } + { + // note1: separate the thread for main control (including navigation) loop to guarantee a relatively stable loop + // rate + ros::TimerOptions ops(ros::Duration(1.0 / main_rate), boost::bind(&AerialRobotBase::mainFunc, this, _1), + &main_loop_queue_); + main_timer_ = nhp_.createTimer(ops); + main_loop_spinner_.start(); + } // note2: callback_spinner_ calls following items with 4 threads // - all subscribers (joint state for robot model, sensor for state estimation, uav/nav for navigation) // - statePublish timer in state estimator for publish odometry and tf // - service server callback_spinner_.start(); - } AerialRobotBase::~AerialRobotBase() { // stop manually to avoid following error (message) - // terminate called after throwing an instance of 'boost::exception_detail::clone_impl >' - // what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument + // terminate called after throwing an instance of + // 'boost::exception_detail::clone_impl >' what(): + // boost: mutex lock failed in pthread_mutex_lock: Invalid argument main_timer_.stop(); main_loop_spinner_.stop(); } -void AerialRobotBase::mainFunc(const ros::TimerEvent & e) +void AerialRobotBase::mainFunc(const ros::TimerEvent& e) { navigator_->update(); controller_->update(); diff --git a/aerial_robot_base/src/aerial_robot_base_node.cpp b/aerial_robot_base/src/aerial_robot_base_node.cpp index e695ff35e..9d10f09b2 100644 --- a/aerial_robot_base/src/aerial_robot_base_node.cpp +++ b/aerial_robot_base/src/aerial_robot_base_node.cpp @@ -1,19 +1,13 @@ #include "aerial_robot_base/aerial_robot_base.h" -int main (int argc, char **argv) +int main(int argc, char** argv) { - ros::init (argc, argv, "aeria_robot_base"); + ros::init(argc, argv, "aerial_robot_base"); ros::NodeHandle nh; ros::NodeHandle nh_private("~"); - AerialRobotBase* aerialRobotBaseNode = new AerialRobotBase(nh, nh_private); + auto* aerialRobotBaseNode = new AerialRobotBase(nh, nh_private); ros::waitForShutdown(); delete aerialRobotBaseNode; return 0; } - - - - - - diff --git a/aerial_robot_base/test/aerial_robot_base/hovering_check.py b/aerial_robot_base/test/aerial_robot_base/hovering_check.py index f6a5489b8..de24cc73f 100755 --- a/aerial_robot_base/test/aerial_robot_base/hovering_check.py +++ b/aerial_robot_base/test/aerial_robot_base/hovering_check.py @@ -42,25 +42,26 @@ from std_msgs.msg import Empty from aerial_robot_msgs.msg import PoseControlPid -PKG = 'rostest' +PKG = "rostest" -class HoveringCheck(): + +class HoveringCheck: def __init__(self, name="hovering_check"): - self.hovering_duration = rospy.get_param('~hovering_duration', 0.0) - self.convergence_thresholds = rospy.get_param('~convergence_thresholds', [0.01, 0.01, 0.01]) # [xy,z, theta] - self.controller_sub = rospy.Subscriber('debug/pose/pid', PoseControlPid, self._controlCallback) + self.hovering_duration = rospy.get_param("~hovering_duration", 0.0) + self.convergence_thresholds = rospy.get_param("~convergence_thresholds", [0.01, 0.01, 0.01]) # [xy,z, theta] + self.controller_sub = rospy.Subscriber("debug/pose/pid", PoseControlPid, self._controlCallback) self.control_msg = None def hoveringCheck(self): # start motor arming - start_pub = rospy.Publisher('teleop_command/start', Empty, queue_size=1) - takeoff_pub = rospy.Publisher('teleop_command/takeoff', Empty, queue_size=1) + start_pub = rospy.Publisher("teleop_command/start", Empty, queue_size=1) + takeoff_pub = rospy.Publisher("teleop_command/takeoff", Empty, queue_size=1) - time.sleep(0.5) # wait for publisher initialization + time.sleep(0.5) # wait for publisher initialization start_pub.publish(Empty()) - time.sleep(1.0) # second + time.sleep(1.0) # second # start takeoff takeoff_pub.publish(Empty()) @@ -68,54 +69,64 @@ def hoveringCheck(self): deadline = rospy.Time.now() + rospy.Duration(self.hovering_duration) while not rospy.Time.now() > deadline: if self.control_msg is not None: - err_x = self.control_msg.x.err_p; - err_y = self.control_msg.y.err_p; - err_z = self.control_msg.z.err_p; - err_yaw = self.control_msg.yaw.err_p; - err_xy = math.sqrt(err_x * err_x + err_y * err_y); - rospy.loginfo_throttle(1, 'errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' % (err_xy, err_x, err_y, err_z, err_yaw)) + err_x = self.control_msg.x.err_p + err_y = self.control_msg.y.err_p + err_z = self.control_msg.z.err_p + err_yaw = self.control_msg.yaw.err_p + err_xy = math.sqrt(err_x * err_x + err_y * err_y) + rospy.loginfo_throttle( + 1, "errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]" % (err_xy, err_x, err_y, err_z, err_yaw) + ) rospy.sleep(1.0) - err_x = self.control_msg.x.err_p; - err_y = self.control_msg.y.err_p; - err_z = self.control_msg.z.err_p; - err_yaw = self.control_msg.yaw.err_p; - err_xy = math.sqrt(err_x * err_x + err_y * err_y); + err_x = self.control_msg.x.err_p + err_y = self.control_msg.y.err_p + err_z = self.control_msg.z.err_p + err_yaw = self.control_msg.yaw.err_p + err_xy = math.sqrt(err_x * err_x + err_y * err_y) # check convergence - if err_xy < self.convergence_thresholds[0] and math.fabs(err_z) < self.convergence_thresholds[1] and math.fabs(err_yaw) < self.convergence_thresholds[2]: + if ( + err_xy < self.convergence_thresholds[0] + and math.fabs(err_z) < self.convergence_thresholds[1] + and math.fabs(err_yaw) < self.convergence_thresholds[2] + ): return True # cannot be convergent - rospy.logerr('errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' % (err_xy, err_x, err_y, err_z, err_yaw)) + rospy.logerr("errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]" % (err_xy, err_x, err_y, err_z, err_yaw)) return False def _controlCallback(self, msg): self.control_msg = msg - err_x = self.control_msg.x.err_p; - err_y = self.control_msg.y.err_p; - err_z = self.control_msg.z.err_p; - err_yaw = self.control_msg.yaw.err_p; - err_xy = math.sqrt(err_x * err_x + err_y * err_y); - rospy.logdebug_throttle(1, 'errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' % (err_xy, err_x, err_y, err_z, err_yaw)) + err_x = self.control_msg.x.err_p + err_y = self.control_msg.y.err_p + err_z = self.control_msg.z.err_p + err_yaw = self.control_msg.yaw.err_p + err_xy = math.sqrt(err_x * err_x + err_y * err_y) + rospy.logdebug_throttle( + 1, "errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]" % (err_xy, err_x, err_y, err_z, err_yaw) + ) + class HoveringTest(unittest.TestCase): def __init__(self, *args): super(self.__class__, self).__init__(*args) rospy.init_node("hovering_test") - self.hovering_delay = rospy.get_param('~hovering_delay', 1.0) + self.hovering_delay = rospy.get_param("~hovering_delay", 1.0) def test_hovering(self): rospy.sleep(self.hovering_delay) checker = HoveringCheck() - self.assertTrue(checker.hoveringCheck(), msg='Cannot reach convergence') + self.assertTrue(checker.hoveringCheck(), msg="Cannot reach convergence") + -if __name__ == '__main__': +if __name__ == "__main__": print("start check hovering") try: - rostest.run(PKG, 'hovering_check', HoveringTest, sys.argv) + rostest.run(PKG, "hovering_check", HoveringTest, sys.argv) except KeyboardInterrupt: pass diff --git a/docs/UMLs/classes.puml b/docs/UMLs/classes.puml new file mode 100644 index 000000000..b144b306c --- /dev/null +++ b/docs/UMLs/classes.puml @@ -0,0 +1,102 @@ +@startuml +'https://plantuml.com/class-diagram + +set namespaceSeparator :: + +abstract class aerial_robot_control::ControlBase { + + ControlBase() + + ~ControlBase() + + initialize() + + update() + + activate() + + reset() +} + +abstract class aerial_robot_control::PoseLinearController extends aerial_robot_control::ControlBase { + + PoseLinearController() + + ~PoseLinearController() + + initialize() + + update() + + reset() +} + +class aerial_robot_control::FullyActuatedController extends aerial_robot_control::PoseLinearController { + + FullyActuatedController() + + ~FullyActuatedController() + + initialize() + + reset() + + controlCore() + + sendCmd() +} + +class aerial_robot_control::UnderActuatedController extends aerial_robot_control::PoseLinearController { + + UnderActuatedController() + + ~UnderActuatedController() + + initialize() + + reset() +} + +class aerial_robot_control::UnderActuatedLQIController extends aerial_robot_control::PoseLinearController { + + UnderActuatedLQIController() + + ~UnderActuatedLQIController() + + initialize() + + activate() +} + +class aerial_robot_control::UnderActuatedTiltedLQIController extends aerial_robot_control::UnderActuatedLQIController { + + UnderActuatedTiltedLQIController() + + ~UnderActuatedTiltedLQIController() + + initialize() + + activate() +} + +class aerial_robot_control::HydrusLQIController extends aerial_robot_control::UnderActuatedLQIController { + + HydrusLQIController() + + initialize() + + checkRobotModel() +} + +class aerial_robot_control::HydrusTiltedLQIController extends aerial_robot_control::UnderActuatedTiltedLQIController { + + HydrusTiltedLQIController() + + initialize() + + checkRobotModel() +} + +class aerial_robot_control::DragonLQIGimbalController extends aerial_robot_control::HydrusLQIController { + + DragonLQIGimbalController() + + ~DragonLQIGimbalController() + + initialize() + + update() + + reset() +} + +class aerial_robot_control::DragonFullVectoringController extends aerial_robot_control::PoseLinearController { + + DragonFullVectoringController() + + ~DragonFullVectoringController() + + initialize() +} + +namespace aerial_robot_control { + note "under-actuated multi-rotor (quad-rotor)" as ud_mr + note "hydrus (option)" as hy + note "fully-actuated multi-rotor (tilted hex-rotor)" as fu_mr + note "hydrus_xi (hex type)" as hy_xi + note "hydrus (non tilted)" as hy_nt + note "dragon" as dragon + note "hydrus (non tilted/ tilted)" as hy_all +} +UnderActuatedController .. ud_mr +UnderActuatedController .. hy +FullyActuatedController .. fu_mr +FullyActuatedController .. hy_xi +HydrusLQIController .. hy_nt +HydrusLQIController .. ud_mr +HydrusTiltedLQIController .. hy_all +DragonLQIGimbalController .. dragon +DragonFullVectoringController .. dragon + + + +'TODO: give a unique namespace to robot specific controllers + +@enduml diff --git a/docs/UMLs/classes.svg b/docs/UMLs/classes.svg new file mode 100644 index 000000000..1a7408e7c --- /dev/null +++ b/docs/UMLs/classes.svg @@ -0,0 +1 @@ +aerial_robot_controlControlBaseControlBase()~ControlBase()initialize()update()activate()reset()PoseLinearControllerPoseLinearController()~PoseLinearController()initialize()update()reset()FullyActuatedControllerFullyActuatedController()~FullyActuatedController()initialize()reset()controlCore()sendCmd()UnderActuatedControllerUnderActuatedController()~UnderActuatedController()initialize()reset()UnderActuatedLQIControllerUnderActuatedLQIController()~UnderActuatedLQIController()initialize()activate()UnderActuatedTiltedLQIControllerUnderActuatedTiltedLQIController()~UnderActuatedTiltedLQIController()initialize()activate()HydrusLQIControllerHydrusLQIController()initialize()checkRobotModel()HydrusTiltedLQIControllerHydrusTiltedLQIController()initialize()checkRobotModel()DragonLQIGimbalControllerDragonLQIGimbalController()~DragonLQIGimbalController()initialize()update()reset()DragonFullVectoringControllerDragonFullVectoringController()~DragonFullVectoringController()initialize()under-actuated multi-rotor (quad-rotor)hydrus (option)fully-actuated multi-rotor (tilted hex-rotor)hydrus_xi (hex type)hydrus (non tilted)dragonhydrus (non tilted/ tilted) \ No newline at end of file diff --git a/docs/UMLs/folder_structure.puml b/docs/UMLs/folder_structure.puml new file mode 100644 index 000000000..c8032c421 --- /dev/null +++ b/docs/UMLs/folder_structure.puml @@ -0,0 +1,123 @@ +@startuml +'https://plantuml.com/component-diagram + +'Legend +' - Blue: common packages +' - Orange: only for simulation +' - Green: only for real machine + +package "jsk_aerial_robot" { + +[aerial_robot_base] #LightBlue +note right of aerial_robot_base + * standalone core + - execute functions from following blocks +end note + +[aerial_robot_model] #LightBlue +note right of [aerial_robot_model] + * joint based modelling + - kinematics, statics, stability + - jacobian + - derive model class as ros plugin + - servo motor interface (bridge) +end note + +[aerial_robot_estimation] #LightBlue +note right of [aerial_robot_estimation] + * sensor fusion + - cog motion estimation + - sensor interface as ros plugin + - kalman filter as ros plugin +end note + +[aerial_robot_control] #LightBlue +note right of [aerial_robot_control] + * flight control / navigation + - control based on cog motion + - navigation for cog and joint motion + - derive control/navigation class as ros plugin +end note + +[aerial_robot_msgs] #LightBlue +note right of [aerial_robot_msgs] + * ros messages + - aerial_robot_msgs/XXXXX +end note + +[aerial_robot_base] -[hidden]-> [aerial_robot_model] +[aerial_robot_model] -[hidden]-> [aerial_robot_estimation] +[aerial_robot_estimation] -[hidden]-> [aerial_robot_control] +[aerial_robot_control] -[hidden]-> [aerial_robot_msgs] + +package "robots" { + + note right of "robots" + * derive class as plugin for: + - model from aerial_robot_model + - control from aerial_robot_control + - navigation from aerial_robot_control (option) + end note + + [hydrus] #LightBlue + note right of [hydrus] + - two dimensional multilinked type + - vertical thrust + end note + + [hydrus_xi] #LightBlue + note right of [hydrus_xi] + - two dimensional multilinked type + - 1D vectorable thrust + end note + + [dragon] #LightBlue + note right of [dragon] + - three dimensional multilinked type + - 2D vectorable thrust + end note + + [hydrus] -[hidden]-> [hydrus_xi] + [hydrus_xi] -[hidden]-> [dragon] +} + +[aerial_robot_simulation] #Orange +note right of [aerial_robot_simulation] + * interface for gazebo + - hardware interface + - controller interface as plugin +end note + +[aerial_robot_msgs] -[hidden]-> [aerial_robot_simulation] + + +package "aerial_robot_nerve" { + [spinal_ros_bridge] #LightGreen + note right of [spinal_ros_bridge] + * uart between ros and spinal + - rosserial based + end note + + [spinal] #LightGreen + note right of [spinal] + * project for stm32 mcu + - attitude estimation + - attitude control + - CAN comm with neuron + end note + + [neuron] #LightGreen + note right of [neuron] + * project for stm32 mcu + - sensor interface via GPIO + - actuator interface via GPIO + - CAN comm with spinal + end note + + [spinal_ros_bridge] -[hidden]-> [spinal] + [spinal] -[hidden]-> [neuron] +} + +} + +@enduml diff --git a/docs/UMLs/folder_structure.svg b/docs/UMLs/folder_structure.svg new file mode 100644 index 000000000..23315f8e5 --- /dev/null +++ b/docs/UMLs/folder_structure.svg @@ -0,0 +1 @@ +jsk_aerial_robotrobotsaerial_robot_nerveaerial_robot_basestandalone core- execute functions from following blocksaerial_robot_modeljoint based modelling- kinematics, statics, stability- jacobian- derive model class as ros plugin- servo motor interface (bridge)aerial_robot_estimationsensor fusion- cog motion estimation- sensor interface as ros plugin- kalman filter as ros pluginaerial_robot_controlflight control / navigation- control based on cog motion- navigation for cog and joint motion- derive control/navigation class as ros pluginaerial_robot_msgsros messages- aerial_robot_msgs/XXXXXaerial_robot_simulationinterface for gazebo- hardware interface- controller interface as pluginderive class as plugin for:- model from aerial_robot_model- control from aerial_robot_control- navigation from aerial_robot_control (option)hydrus- two dimensional multilinked type- vertical thrusthydrus_xi- two dimensional multilinked type- 1D vectorable thrustdragon- three dimensional multilinked type- 2D vectorable thrustspinal_ros_bridgeuart between ros and spinal- rosserial basedspinalproject for stm32 mcu- attitude estimation- attitude control- CAN comm with neuronneuronproject for stm32 mcu- sensor interface via GPIO- actuator interface via GPIO- CAN comm with spinal \ No newline at end of file diff --git a/docs/UMLs/system_config.puml b/docs/UMLs/system_config.puml new file mode 100644 index 000000000..4ad69da48 --- /dev/null +++ b/docs/UMLs/system_config.puml @@ -0,0 +1,74 @@ +@startuml +'https://plantuml.com/component-diagram + + +package "Class Member as Plugins" { + [robot_model_loader_("aerial_robot_model",\n "aerial_robot_model::RobotModel")] as model_plugin + [pluginlib::ClassLoader(aerial_robot_estimation",\n "sensor_plugin::SensorBase")] as sensor_plugin + [pluginlib::ClassLoader("kalman_filter",\n "kf_plugin::KalmanFilter"))] as kf_plugin + model_plugin -[hidden]-> sensor_plugin + sensor_plugin -[hidden]-> kf_plugin +} + +package "Load as Plugin" { + [navigator_loader_("aerial_robot_control",\n "aerial_robot_navigation::BaseNavigator")] as navigator_plugin + [controller_loader_("aerial_robot_control",\n "aerial_robot_control::ControlBase")] as controller_plugin + navigator_plugin -[hidden]-> controller_plugin +} + +component "aerial_robot_base" as base { + [ROS spinal (ros::AsyncSpinner)\n4 multi-thread to call callbacks] as ROS_Spinal + [*Timer Loop\lVoid mainFunc(const ros::TimerEvent & e)\l{\l navigator_->update();\l controller_->update();\l }] as TimerLoop + package Instance { + [robot_model_ros_] + [estimator_] + [navigator_] + [controller_] + } +} + +[servo_bridge] + +package "Simulation (Gazebo)" { + ["Hardware interface"] + ["Controller interfaces"] + ["Hardware interface"] -[hidden]-> ["Controller interfaces"] +} + +package "Real Machine" { + [spinal_ros_bridge] + [sensor interfaces] + [spinal] +} + +'Relationship + +model_plugin -> robot_model_ros_ +sensor_plugin -> estimator_ +kf_plugin -> estimator_ + +navigator_plugin -> navigator_ +controller_plugin -> controller_ + +robot_model_ros_ --> estimator_ +robot_model_ros_ --> navigator_ +robot_model_ros_ --> controller_ +estimator_ --> navigator_ +estimator_ --> controller_ +navigator_ --> controller_ + +navigator_ -> TimerLoop +controller_ -> TimerLoop + +Instance --> ROS_Spinal + +base <--> servo_bridge +base <--> spinal_ros_bridge +servo_bridge <--> spinal_ros_bridge +servo_bridge <--> "Simulation (Gazebo)" +spinal_ros_bridge <--> spinal +[sensor interfaces] --> base + +kf_plugin -[hidden]-> navigator_plugin + +@enduml diff --git a/docs/UMLs/system_config.svg b/docs/UMLs/system_config.svg new file mode 100644 index 000000000..e86d5a205 --- /dev/null +++ b/docs/UMLs/system_config.svg @@ -0,0 +1 @@ +Class Member as PluginsLoad as Pluginaerial_robot_baseInstanceSimulation (Gazebo)Real Machinerobot_model_loader_("aerial_robot_model","aerial_robot_model::RobotModel")pluginlib::ClassLoader(aerial_robot_estimation","sensor_plugin::SensorBase")pluginlib::ClassLoader("kalman_filter","kf_plugin::KalmanFilter"))navigator_loader_("aerial_robot_control","aerial_robot_navigation::BaseNavigator")controller_loader_("aerial_robot_control","aerial_robot_control::ControlBase")ROS spinal (ros::AsyncSpinner)4 multi-thread to call callbacksTimer LoopVoid mainFunc(const ros::TimerEvent & e){navigator_->update();controller_->update();}robot_model_ros_estimator_navigator_controller_Hardware interfaceController interfacesspinal_ros_bridgesensor interfacesspinalservo_bridge \ No newline at end of file diff --git a/docs/UMLs/workflow.puml b/docs/UMLs/workflow.puml new file mode 100644 index 000000000..38c4b358e --- /dev/null +++ b/docs/UMLs/workflow.puml @@ -0,0 +1,48 @@ +@startuml +'https://plantuml.com/sequence-diagram + +' Legend +' ->> means inner communication via variables +'-> means outer communication via ROS topics + +'for nested boxes +!pragma teoz true + +autonumber + +box Laptop #Khaki +participant RViz +participant Terminal +end box + +box "Onboard PC\n(Jetson NX)" #LightGreen +participant "Nav" as Nav +participant "Control" as Ctl +participant "Estimator" as Est +end box + +box Gazebo (Laptop) #Aliceblue + + box " MCU\n(STM32 H7)" #LightBlue + participant "Spinal/Control_Spawner" as Spinal + end box + + box Body #LightGray + participant "ESC&Servo" as ESC + end box + +end box + +Est ->> Est: 100Hz + +loop 40Hz + Est ->> Ctl: estimator_->getState() +end loop + +Nav ->> Ctl: target_pos/vel/acc +loop 10Hz + Ctl -> Spinal: qd/attitude\ngeometry_msgs/Vector3Stamped +end loop +Spinal -> ESC: PWM + +@enduml diff --git a/docs/UMLs/workflow.svg b/docs/UMLs/workflow.svg new file mode 100644 index 000000000..56fc47d67 --- /dev/null +++ b/docs/UMLs/workflow.svg @@ -0,0 +1 @@ +LaptopOnboard PC(Jetson NX)Gazebo (Laptop)MCU(STM32 H7)BodyRVizTerminalNavControlEstimatorSpinal/Control_SpawnerESC&ServoRVizTerminalNavControlEstimatorSpinal/Control_SpawnerESC&Servo1100Hzloop[40Hz]2estimator_->getState()3target_pos/vel/accloop[10Hz]4qd/attitudegeometry_msgs/Vector3Stamped5PWM \ No newline at end of file diff --git a/docs/about_our_system.md b/docs/about_our_system.md new file mode 100644 index 000000000..f2426b346 --- /dev/null +++ b/docs/about_our_system.md @@ -0,0 +1,108 @@ +# About Our System + +## Folder Structure + +![](./UMLs/folder_structure.svg) + +## System Config + +![](./UMLs/system_config.svg) + +## Class Diagram + +![](./UMLs/classes.svg) + +## Workflow + +![](./UMLs/workflow.svg) + +## File Structure + +### ./aerial_robot + +A ROS Package. A sum-up of all the packages in this repository. + +### ./aerial_robot_base + +A ROS Package. Aerial robot base class. It contains: +- bin: some bash commands +- config: parameters for sensors, including altimeter, gps, imu, and mocap. Stored in .yaml files. +- include: header files for the aerial_robot_base class. +- launch: launch files for some common functions, including joy_stick, mocap, and rtk_gps. +- scripts: python files for some common functions, including controlling the robot with a keyboard and calculating RMS error. +- src: source files for the aerial_robot_base class, loading plugins for robot_model, estimator, navigator and controller. + Define a main loop with main_rate and update navigator and controller subsequently. +- test: files for rostest, only hovering test. + +### ./aerial_robot_control + +A ROS Package with various plugins for control. It contains: +- cfg: python files for dynamic reconfigure, a tool to change parameters online. Include PID and LQI controllers. +- include: header files for the controller. +- plugins: controller plugins, including FullyActuatedController, UnderActuatedController, UnderActuatedLQIController, + and UnderActuatedTiltedLQIController. User can select one of them through parameters, which generally stored in FlightControl.yaml. +- scripts: python demo to test controller. +- src: source files for the controllers. + +### ./aerial_robot_estimation + +A ROS package with various plugins for sensor usage and state estimation. It contains: +- cfg: python files for dynamic reconfigure, including Kalman Filter. +- config: parameters for optical flow. +- include: header files for the estimator. +- launch: launch files for the optical flow. +- plugins: estimator plugins, including kf_plugins (choosing sensor fusion method), sensor_plugins (choosing sensors), + and and vision_nodelet. Parameters are generally stored in StateEstimation.yaml. +- src: source files for the estimator: kalman filter for sensor fusion, different sensors, and vision (optical flow only). + Sensors include altitude, gps, imu, mocap, plane_detection and visual odometry (VO). + +### ./aerial_robot_model + +A ROS package with various plugins for robot model, calculating kinematics from urdf parameters with KDL and publishing +tf info. It contains: +- include: header files for the robot model. +- launch: launch files for setting robot_description to rosparameter server, publishing joint state and tf info, and + visualizing the robot in rviz. +- nodes: don't know yet. Maybe lisp file? +- plugins: include multirotor_robot_model and underactuated_tilted_robot_model. +- script: a python file that converts cog and intertia parameters from 3D models. +- src: source files for computing kinematics automatically, communicating with servos, and publishing tf info. +- srv: a service for adding a new link to the robot model. +- test: test by comparing jacobians calculated by KDL and numerical method. + +### ./aerial_robot_msgs + +A ROS package defining message types for aerial robot, containing: state-based info, parameter-based info. + +### ./aerial_robot_nerve + +Program for low-level autopilot running on a STM32 MCU. Leverage STM32CubeMX to generate the code and rosserial to communicate +with ROS. + +### ./aerial_robot_simulation + +A ROS package with various plugins for simulation in Gazebo, simulating dynamics, sensor noise, and controller in the nerve module. It contains: +- config: parameters for simulation, including the parameters to simulating MoCap. Stored in .yaml files. +- gazebo_model: world files. +- include: header files for the simulation. +- launch: launch files for Gazebo-based simulation. +- scripts: gazebo_control_bridge.py +- src: source files for the simulation, including dynamics, sensor noise, and controller in the nerve module. +- xacro: sensor setting for spinal MCU + +### ./images + +Store images. + +### ./robots + +Include multiple ROS Packages. Each package is for one specific robot. Take mini_quadrotor as an example, it contains: +- config: the yaml files for the robot, including various parameters for the robot, such as the parameters for the + motors, controllers, rviz, simulation, and so on. +- launch: the launch files for the robot. Note that for each robot, there is a bringup.launch file in the launch + folder. The bringup.launch file will load parameters and launch other launch files, including motor, battery, state + fusion, control, navigation, and so on. +- test: files for rostest. Hovering test included. +- urdf: the urdf files for the robot, including the dae files (shape and inertial) for main_body, propeller, and a + battery. Based on these urdf files, the kinematic parameters of the whole body will be calculated through Kinematics + and Dynamics Library (KDL). The urdf files are also used for visualization in rviz and Gazebo. diff --git a/docs/beginning.md b/docs/beginning.md new file mode 100644 index 000000000..94b6c9c58 --- /dev/null +++ b/docs/beginning.md @@ -0,0 +1,5 @@ +# How to begin + +1. learn Markdown +2. learn Git +3. learn GitHub \ No newline at end of file diff --git a/docs/how_to_contribute.md b/docs/how_to_contribute.md new file mode 100644 index 000000000..e9dde7bef --- /dev/null +++ b/docs/how_to_contribute.md @@ -0,0 +1,19 @@ +# How to contribute + +## Contribute Process + +1. fork the original repository from https://github.com/jsk-ros-pkg/jsk_aerial_robot +2. develop your own feature in your forked repository +3. once you finish your development, make a pull request to the original repository +4. the original repository maintainer will review your pull request and merge it if it is OK + +## Code Style + +We use Clang-format to format the C++ code, and use black to format the Python code. +Pre-commit, the format plugin, will run automatically before each commit. The specific function has been written in +.pre-commit-config.yaml. To activate this plugin, please run the following command in terminal and root directory: + +```bash + pip3 install pre-commit + pre-commit install +``` diff --git a/robots/mini_quadrotor/README.md b/robots/mini_quadrotor/README.md new file mode 100644 index 000000000..a0514bf8a --- /dev/null +++ b/robots/mini_quadrotor/README.md @@ -0,0 +1,15 @@ +# Mini Quadrotor + +# How to use + +In one terminal, run + +`roslaunch mini_quadrotor bringup.launch real_machine:=false simulation:=True headless:=False` + +In another terminal, run + +`rosrun aerial_robot_base keyboard_command.py` + +In this terminal, input `r` to arm the quadrotor, then input `t` to takeoff. The quadrotor will takeoff and hover. + +Input `l` to land the quadrotor.