Merging mavros interface updates

interfaces/kr_betaflight_interface/config/neurofly.yaml

@@ -3,21 +3,22 @@ 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
 
-## 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
+## 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

interfaces/kr_betaflight_interface/config/trackers.yaml

@@ -1,10 +1,13 @@
 /neurofly1/trackers_manager:
   ros__parameters:
-    trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker"]
+    trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker", "CircleTracker", "LissajousTracker", "LissajousAdder"]
+    circle_tracker/ramp_up_time: 2.0
     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
+    lissajous_tracker/frame_id: odom
+    lissajous_adder/frame_id: odom

interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp

@@ -348,10 +348,10 @@ CrsfMsg CrsfBridge::generateCrsfMessageFromSO3Command(const kr_mav_msgs::msg::SO
 
   // remap throttle (1000 to 2000) to crsf (kMinCmd to kMaxCmd)
   uint16_t throttle_cmd = round(((throttle - 1000) / 1000) * (CrsfMsg::kMaxCmd - CrsfMsg::kMinCmd) + CrsfMsg::kMinCmd);
-  RCLCPP_INFO_THROTTLE(
-      logger_, *node_->get_clock(), 1000,
-      "AUTONOMOUS MODE: thrust: %f throttle: %f throttle_cmd: %d",
-      thrust, throttle, throttle_cmd);
+//   RCLCPP_INFO_THROTTLE(
+//       logger_, *node_->get_clock(), 1000,
+//       "AUTONOMOUS MODE: thrust: %f throttle: %f throttle_cmd: %d",
+//       thrust, throttle, throttle_cmd);
   crsf_msg.setThrottleCommand(throttle_cmd);
 
   // convert quaternion to euler

interfaces/kr_mavros_interface/CMakeLists.txt

@@ -1,62 +1,63 @@
-cmake_minimum_required(VERSION 3.5)
-project(kr_mavros_interface)
-
-# set default build type
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE RelWithDebInfo)
-endif()
-
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-add_compile_options(-Wall)
-
-# ROS 2 configuration
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(kr_mav_msgs REQUIRED)
-find_package(mavros_msgs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(Eigen3 REQUIRED)
-
-# If mavros_msgs not found, don't build but do not cause a build error
-if(NOT mavros_msgs_FOUND)
-  message(WARNING "NOTE: mavros_msgs not found so not building kr_mavros_interface")
-else()
-  # Create shared library
-  add_library(${PROJECT_NAME} SHARED src/so3cmd_to_mavros_nodelet.cpp)
-
-  ament_target_dependencies(${PROJECT_NAME}
-    rclcpp
-    rclcpp_components
-    nav_msgs
-    geometry_msgs
-    sensor_msgs
-    kr_mav_msgs
-    mavros_msgs
-    tf2
-    tf2_geometry_msgs
-    Eigen3)
-
-  target_include_directories(${PROJECT_NAME} PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>)
-
-  # Register component
-  rclcpp_components_register_nodes(${PROJECT_NAME} "kr_mavros_interface::SO3CmdToMavros")
-
-  # Install
-  install(TARGETS ${PROJECT_NAME}
-    ARCHIVE DESTINATION lib
-    LIBRARY DESTINATION lib
-    RUNTIME DESTINATION bin)
-
-  install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
-endif()
-
-ament_package()
+cmake_minimum_required(VERSION 3.5)
+project(kr_mavros_interface)
+
+# set default build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebInfo)
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+add_compile_options(-Wall)
+
+# ROS 2 configuration
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(kr_mav_msgs REQUIRED)
+find_package(mavros_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+# If mavros_msgs not found, don't build but do not cause a build error
+if(NOT mavros_msgs_FOUND)
+  message(WARNING "NOTE: mavros_msgs not found so not building kr_mavros_interface")
+else()
+  # Create shared library
+  add_library(${PROJECT_NAME} SHARED src/so3cmd_to_mavros_nodelet.cpp)
+
+  ament_target_dependencies(${PROJECT_NAME}
+    rclcpp
+    rclcpp_components
+    nav_msgs
+    geometry_msgs
+    sensor_msgs
+    kr_mav_msgs
+    mavros_msgs
+    tf2
+    tf2_geometry_msgs
+    Eigen3)
+
+  target_include_directories(${PROJECT_NAME} PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
+  # Register component
+  rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToMavros")
+
+  # Install
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin)
+
+  install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
+  install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config)
+endif()
+
+ament_package()

interfaces/kr_mavros_interface/config/neurofly.yaml

@@ -0,0 +1,17 @@
+/**/so3cmd_to_mavros:
+  ros__parameters:
+
+    num_props: 4
+    thrust_vs_rpm_cof_a: 1.31212977e-08
+    thrust_vs_rpm_cof_b: -4.38553e-05
+    thrust_vs_rpm_cof_c: 7.89533392e-02
+
+    lin_cof_a: 5.681818181818182e-05
+    lin_int_b: -0.09801136363636374
+
+    so3_cmd_timeout: 0.25    
+    odom_timeout: 1.0
+
+    idle_hold_max_force: 1.0
+
+    vision_pose_rate: 50.0

interfaces/kr_mavros_interface/launch/test.launch.py → interfaces/kr_mavros_interface/launch/mavros_interface.launch.py

@@ -1,35 +1,39 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
+    # Airframe properties (thrust curve, timeouts, vision_pose rate) live in the
+    # config file rather than being duplicated as launch arguments here and in
+    # neurofly_interface/launch/system_launch.launch.py. Override the file with
+    # config_file:= to fly a different airframe.
+    default_config_file = os.path.join(
+        get_package_share_directory("kr_mavros_interface"),
+        "config",
+        "neurofly.yaml",
+    )
+
     # Declare launch arguments
     robot_arg = DeclareLaunchArgument("robot", default_value="/", description="Robot namespace")
     odom_arg = DeclareLaunchArgument("odom", default_value="odom", description="Odometry topic")
     so3_cmd_arg = DeclareLaunchArgument("so3_cmd", default_value="so3_cmd", description="SO3 command topic")
-    num_props_arg = DeclareLaunchArgument("num_props", default_value="4", description="Number of propellers")
-    kf_arg = DeclareLaunchArgument("kf", default_value="2.137145e-6", description="Thrust coefficient")
-    lin_cof_a_arg = DeclareLaunchArgument("lin_cof_a", default_value="0.0015", description="Linear coefficient A")
-    lin_int_b_arg = DeclareLaunchArgument("lin_int_b", default_value="-1.5334", description="Linear intercept B")
+    config_file_arg = DeclareLaunchArgument(
+        "config_file", default_value=default_config_file, description="SO3CmdToMavros parameter file"
+    )
 
     # Create composable node
     so3_cmd_to_mavros_node = ComposableNode(
         package="kr_mavros_interface",
         plugin="SO3CmdToMavros",
         name="so3cmd_to_mavros",
         namespace=LaunchConfiguration("robot"),
-        parameters=[
-            {
-                "num_props": LaunchConfiguration("num_props"),
-                "kf": LaunchConfiguration("kf"),
-                "lin_cof_a": LaunchConfiguration("lin_cof_a"),
-                "lin_int_b": LaunchConfiguration("lin_int_b"),
-                "so3_cmd_timeout": 0.25,
-            }
-        ],
+        parameters=[LaunchConfiguration("config_file")],
         remappings=[
             ("~/odom", LaunchConfiguration("odom")),
             ("~/so3_cmd", LaunchConfiguration("so3_cmd")),
@@ -54,10 +58,7 @@ def generate_launch_description():
             robot_arg,
             odom_arg,
             so3_cmd_arg,
-            num_props_arg,
-            kf_arg,
-            lin_cof_a_arg,
-            lin_int_b_arg,
+            config_file_arg,
             container,
         ]
     )