Merged ros2 humble dev from Neurofly

'

@@ -0,0 +1,105 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+import os
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import GroupAction
+from launch_ros.actions import PushRosNamespace
+
+def generate_launch_description():
+
+  ld = LaunchDescription()
+  odom_topic = LaunchConfiguration('odom')
+  so3_cmd_topic = LaunchConfiguration('so3_cmd')
+
+  robot_arg = DeclareLaunchArgument(
+    'robot', default_value='quadrotor'
+  )
+  odom_arg = DeclareLaunchArgument(
+    'odom', default_value='odom'
+  )
+  so3_cmd_arg = DeclareLaunchArgument(
+    'so3_cmd', default_value='so3_cmd'
+  )
+
+  # Path to the configuration file
+  config_file = FindPackageShare('kr_crazyflie_interface').find('kr_crazyflie_interface') + '/config/crazyflie.yaml'
+
+  mav_services_node = Node(
+      package="kr_mav_manager",
+      executable="mav_services",
+      namespace=LaunchConfiguration('robot'),
+      name="mav_services",
+      output='screen'
+  )
+
+  rqt_gui_node = Node(
+      package="rqt_mav_manager",
+      executable="rqt_mav_manager",
+      namespace=LaunchConfiguration('robot'),
+      name="rqt_mav",
+      output='screen',
+  )
+
+  trackers_manager_config_file = FindPackageShare('kr_trackers_manager').find('kr_trackers_manager') + '/config/trackers_manager.yaml'
+  print(trackers_manager_config_file)
+
+  trackers_manager_launch_path = os.path.join(
+        FindPackageShare('kr_trackers_manager').find('kr_trackers_manager'),
+        'launch',
+        'example.launch.py'
+  )
+
+  # Include the trackers manager launch file
+  trackers_component = IncludeLaunchDescription(
+      PythonLaunchDescriptionSource(trackers_manager_launch_path),
+      launch_arguments={
+          'trackers_manager_config_file': config_file
+          }.items()
+  )
+
+  trackers_component_with_namespace = GroupAction(
+     actions=[
+         PushRosNamespace(LaunchConfiguration('robot')),
+         trackers_component,
+      ]
+   ) 
+
+  # Component configuration
+  so3cmd_to_crazyflie_component = ComposableNodeContainer(
+    name="so3_container",
+    namespace=LaunchConfiguration('robot'),
+    package="rclcpp_components",
+    executable="component_container",
+    composable_node_descriptions=[
+      ComposableNode(
+        package="kr_crazyflie_interface",
+        plugin="SO3CmdToCrazyflie",
+        name="so3cmd_to_crazyflie",
+        parameters=[config_file],
+        remappings=[
+          ("~/odom", odom_topic),
+          ("~/so3_cmd", so3_cmd_topic)
+        ]
+      )
+    ],
+    output='screen'
+  )
+    # Regular Node (not in container)
+
+
+  return LaunchDescription([
+      robot_arg,
+      # odom_arg,
+      # so3_cmd_arg,
+      # so3cmd_to_crazyflie_component,
+      # mav_services_node,
+      # rqt_gui_node,
+      trackers_component_with_namespace,
+
+  ])

.github/workflows/cpplint.yml

@@ -15,6 +15,8 @@ jobs:
           ,-whitespace/braces\
           ,-whitespace/parens\
           ,-whitespace/newline\
+          ,-build/header_guard\
+          ,-readability/todo\
           ,-build/c++11"
         #   -readability/braces\
         #   ,-whitespace/comments\

interfaces/kr_betaflight_interface/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.10)
+project(kr_betaflight_interface)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+include_directories(
+  include
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(kr_mav_msgs REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(tf2 REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/so3cmd_to_betaflight_component.cpp
+  src/crsf/crsf_bridge.cpp
+  src/crsf/crsf_msg.cpp
+  src/crsf/crsf_serial_port.cpp
+  src/sbus/sbus_bridge.cpp
+  src/sbus/sbus_msg.cpp
+  src/sbus/sbus_serial_port.cpp
+)
+ament_target_dependencies(${PROJECT_NAME} 
+  rclcpp
+  rclcpp_components
+  kr_mav_msgs
+  nav_msgs
+  sensor_msgs
+  geometry_msgs
+  tf2)
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
+rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToBetaflight")
+
+# Install headers
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

interfaces/kr_betaflight_interface/config/neurofly.yaml

@@ -0,0 +1,34 @@
+port_name: /dev/ttyTHS1
+protocol_type: crsf # crsf, sbus
+control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout'
+# set in the 'flight_controller'!)
+rc_timeout: 0.1 # [s]
+disable_thrust_mapping: true
+use_body_rates: false
+
+## fpvcycle 2206 motors 4S
+# thrust_vs_rpm_cof_a_: 1.755e-06 # gf
+# thrust_vs_rpm_cof_b_: -6.521e-03 # gf
+# thrust_vs_rpm_cof_c_: 1.388e+01 # gf
+thrust_vs_rpm_cof_a_: 1.72106707e-8 # N
+thrust_vs_rpm_cof_b_: -0.00006394916 # N
+thrust_vs_rpm_cof_c_: 0.136116302 # N
+rpm_vs_throttle_linear_coeff_a_: 2.89518953e+01
+rpm_vs_throttle_linear_coeff_b_: -3.04658581e+04
+rpm_vs_throttle_quadratic_coeff_a_: -2.13455563e-02
+rpm_vs_throttle_quadratic_coeff_b_: 9.61348839e+01
+rpm_vs_throttle_quadratic_coeff_c_: -8.20339135e+04
+
+# Maximum values for body rates and roll and pitch angles as they are set
+# on the Flight Controller. The max roll an pitch angles are only active
+# when flying in angle mode
+max_roll_rate: 1000.0 # [deg/s]
+max_pitch_rate: 1000.0 # [deg/s]
+max_yaw_rate: 1000.0 # [deg/s]
+max_roll_angle: 50.0 # [deg]
+max_pitch_angle: 50.0 # [deg]
+alpha_vbat_filter: 0.01
+perform_thrust_voltage_compensation: false
+thrust_ratio_voltage_map_a: -0.17044342 # [1/V]
+thrust_ratio_voltage_map_b: 3.10495276 # [-]
+n_lipo_cells: 3 # [-]

interfaces/kr_betaflight_interface/config/neurofly_1804.yaml

@@ -0,0 +1,35 @@
+port_name: /dev/ttyTHS1
+protocol_type: crsf # crsf, sbus
+control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout'
+# set in the 'flight_controller'!)
+rc_timeout: 0.1 # [s]
+mass: 0.68 # [kg]
+disable_thrust_mapping: true
+use_body_rates: false
+
+## hyperlite 1804 motors 4S
+# thrust_vs_rpm_cof_a_: 1.338e-06 # gf
+# thrust_vs_rpm_cof_b_: -4.472e-3 # gf
+# thrust_vs_rpm_cof_c_: 8.051 # gf
+thrust_vs_rpm_cof_a_: 1.31212977e-08 # N
+thrust_vs_rpm_cof_b_: -4.38553e-05 # N
+thrust_vs_rpm_cof_c_: 7.89533392e-02 # N
+rpm_vs_throttle_linear_coeff_a_: 17.6
+rpm_vs_throttle_linear_coeff_b_: -15875.0
+rpm_vs_throttle_quadratic_coeff_a_: -30169.81
+rpm_vs_throttle_quadratic_coeff_b_: 35.43775
+rpm_vs_throttle_quadratic_coeff_c_: -0.004962819
+
+# Maximum values for body rates and roll and pitch angles as they are set
+# on the Flight Controller. The max roll an pitch angles are only active
+# when flying in angle mode
+max_roll_rate: 1000.0 # [deg/s]
+max_pitch_rate: 1000.0 # [deg/s]
+max_yaw_rate: 1000.0 # [deg/s]
+max_roll_angle: 50.0 # [deg]
+max_pitch_angle: 50.0 # [deg]
+alpha_vbat_filter: 0.01
+perform_thrust_voltage_compensation: false
+thrust_ratio_voltage_map_a: -0.17044342 # [1/V]
+thrust_ratio_voltage_map_b: 3.10495276 # [-]
+n_lipo_cells: 3 # [-]

interfaces/kr_betaflight_interface/config/neurofly_gains.yaml

@@ -0,0 +1,32 @@
+# neurofly1
+quadrotor_name: neurofly1
+mass: 0.68
+
+kp_x: 6.0
+kp_y: 6.0
+kp_z: 8.0
+kd_x: 3.0
+kd_y: 3.0
+kd_z: 3.5
+ki_x: 0.00
+ki_y: 0.00
+ki_z: 0.00
+
+kib_x: 0.00
+kib_y: 0.00
+kib_z: 0.00
+
+rot_x: 1.2
+rot_y: 1.2
+rot_z: 1.0
+
+ang_x: 0.13
+ang_y: 0.13
+ang_z: 0.1
+
+kf_correction: 0.0e-08
+roll_correction: 0.0
+pitch_correction: 0.0
+
+max_pos_int:   0.3
+max_pos_int_b: 0.3

interfaces/kr_qualcomm_interface/config/tracker_params.yaml → interfaces/kr_betaflight_interface/config/tracker_params.yaml

@@ -1,19 +1,19 @@
 # This should contain tracker parameters
 
 line_tracker_distance:
-  default_v_des: 1.0
-  default_a_des: 0.5
+  default_v_des: 0.2
+  default_a_des: 0.2
   epsilon: 0.1
 
 line_tracker_min_jerk:
   default_v_des: 1.0
   default_a_des: 0.5
-  default_yaw_v_des: 0.8
-  default_yaw_a_des: 0.2
+  default_yaw_v_des: 0.4
+  default_yaw_a_des: 0.3
 
 trajectory_tracker:
-  max_vel_des: 0.5
-  max_acc_des: 0.3
+  max_vel_des: 2.0
+  max_acc_des: 2.0
 
 velocity_tracker:
   timeout: 0.5

interfaces/kr_betaflight_interface/config/trackers.yaml

@@ -0,0 +1,10 @@
+/neurofly1/trackers_manager:
+  ros__parameters:
+    trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker"]
+    line_tracker_distance/default_v_des: 0.2
+    line_tracker_distance/default_a_des: 0.2
+    line_tracker_distance/epsilon: 0.1
+    line_tracker_min_jerk/default_v_des: 1.0
+    line_tracker_min_jerk/default_a_des: 0.5
+    line_tracker_min_jerk/default_yaw_v_des: 0.4
+    line_tracker_min_jerk/default_yaw_a_des: 0.3