Merge branch 'main' into documentation

rosplane/CMakeLists.txt

@@ -33,59 +33,75 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 find_package(rosflight_msgs REQUIRED)
 pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
 
-ament_export_dependencies(rclcpp rclpy)
-ament_export_include_directories(include)
-
-#install(DIRECTORY originalFiles DESTINATION share/${PROJECT_NAME}/)
-include_directories(  #use this if you need .h files for include statements.  The include will need to have the directories where each .h is respectively.
+include_directories(
   include
+  include/param_manager
   ${EIGEN3_INCLUDE_DIRS}
   ${GAZEBO_INCLUDE_DIRS}
   ${YAML_CPP_INCLUDEDIR}
 )
 
+### EXTRA FILES TO INSTALL ###
+
 install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME}/)
 
-### START OF REAL EXECUTABLES ###
+### LIBRARIES ###
+
+# Param Manager
+add_library(param_manager
+  include/param_manager/param_manager.hpp
+  src/param_manager/param_manager.cpp
+)
+ament_target_dependencies(param_manager rclcpp)
+ament_export_targets(param_manager HAS_LIBRARY_TARGET)
+install(DIRECTORY include/param_manager DESTINATION include)
+install(TARGETS param_manager
+  EXPORT param_manager
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+### START OF EXECUTABLES ###
 
 # Controller
 add_executable(rosplane_controller
-            src/controller_base.cpp
-            src/controller_state_machine.cpp
-            src/controller_successive_loop.cpp
-            src/controller_total_energy.cpp
-            src/param_manager.cpp)
+  src/controller_base.cpp
+  src/controller_state_machine.cpp
+  src/controller_successive_loop.cpp
+  src/controller_total_energy.cpp)
 ament_target_dependencies(rosplane_controller rosplane_msgs rosflight_msgs rclcpp rclpy Eigen3)
+target_link_libraries(rosplane_controller param_manager)
 install(TARGETS
   rosplane_controller
   DESTINATION lib/${PROJECT_NAME})
 
-
 # Follower
 add_executable(rosplane_path_follower
-            src/path_follower_example.cpp
-            src/path_follower_base.cpp
-            src/param_manager.cpp)
+  src/path_follower_example.cpp
+  src/path_follower_base.cpp)
 ament_target_dependencies(rosplane_path_follower rosplane_msgs rclcpp rclpy Eigen3)
+target_link_libraries(rosplane_path_follower param_manager)
 install(TARGETS
   rosplane_path_follower
   DESTINATION lib/${PROJECT_NAME})
 
 # Manager
 add_executable(rosplane_path_manager
-            src/path_manager_base.cpp
-            src/path_manager_example.cpp
-            src/param_manager.cpp)
+  src/path_manager_base.cpp
+  src/path_manager_example.cpp)
 ament_target_dependencies(rosplane_path_manager rosplane_msgs rclcpp rclpy Eigen3)
+target_link_libraries(rosplane_path_manager param_manager)
 install(TARGETS
   rosplane_path_manager
   DESTINATION lib/${PROJECT_NAME})
 
 # Planner
 add_executable(rosplane_path_planner
-            src/path_planner.cpp
-            src/param_manager.cpp)
+  src/path_planner.cpp)
 target_link_libraries(rosplane_path_planner
+  param_manager
   ${YAML_CPP_LIBRARIES}
 )
 ament_target_dependencies(rosplane_path_planner rosplane_msgs rosflight_msgs std_srvs rclcpp rclpy Eigen3)
@@ -95,13 +111,16 @@ install(TARGETS
 
 # Estimator
 add_executable(rosplane_estimator_node
-              src/estimator_base.cpp
-              src/estimator_example.cpp
-              src/param_manager.cpp)
+  src/estimator_base.cpp
+  src/estimator_example.cpp)
+target_link_libraries(rosplane_estimator_node
+  ${YAML_CPP_LIBRARIES}
+)
 ament_target_dependencies(rosplane_estimator_node rosplane_msgs rosflight_msgs rclcpp Eigen3)
+target_link_libraries(rosplane_estimator_node param_manager)
 install(TARGETS
-        rosplane_estimator_node
-        DESTINATION lib/${PROJECT_NAME})
+  rosplane_estimator_node
+  DESTINATION lib/${PROJECT_NAME})
 
 #### END OF EXECUTABLES ###
 

rosplane/include/controller_base.hpp

@@ -12,8 +12,7 @@
 
 #include <chrono>
 #include <cstring>
-#include <iostream>
-#include <variant>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rosflight_msgs/msg/command.hpp>
 #include <rosplane_msgs/msg/controller_commands.hpp>

rosplane/include/estimator_base.hpp

@@ -13,7 +13,8 @@
 #include <chrono>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <math.h>
-#include <numeric>
+#include <yaml-cpp/yaml.h>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rosflight_msgs/msg/airspeed.hpp>
 #include <rosflight_msgs/msg/barometer.hpp>
@@ -88,6 +89,7 @@ class estimator_base : public rclcpp::Node
   double init_lat_ = 0.0;                 /**< Initial latitude in degrees */
   double init_lon_ = 0.0;                 /**< Initial longitude in degrees */
   float init_alt_ = 0.0;                  /**< Initial altitude in meters above MSL  */
+  float init_static_;                     /**< Initial static pressure (mbar)  */
 
 private:
   rclcpp::Publisher<rosplane_msgs::msg::State>::SharedPtr vehicle_state_pub_;
@@ -99,11 +101,20 @@ class estimator_base : public rclcpp::Node
   rclcpp::Subscription<rosflight_msgs::msg::Airspeed>::SharedPtr airspeed_sub_;
   rclcpp::Subscription<rosflight_msgs::msg::Status>::SharedPtr status_sub_;
 
+  std::string param_filepath_ = "estimator_params.yaml";
+
   void update();
   void gnssFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
   void gnssVelCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
   void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
   void baroAltCallback(const rosflight_msgs::msg::Barometer::SharedPtr msg);
+  /**
+   * @brief This saves parameters to the param file for later use.
+   *
+   * @param param_name The name of the parameter.
+   * @param param_val The value of the parameter.
+   */
+  void saveParameter(std::string param_name, double param_val);
   void airspeedCallback(const rosflight_msgs::msg::Airspeed::SharedPtr msg);
   void statusCallback(const rosflight_msgs::msg::Status::SharedPtr msg);
 
@@ -119,7 +130,6 @@ class estimator_base : public rclcpp::Node
 
   bool gps_new_;
   bool armed_first_time_;                 /**< Arm before starting estimation  */
-  float init_static_;                     /**< Initial static pressure (mbar)  */
   int baro_count_;                        /**< Used to grab the first set of baro measurements */
   std::vector<float> init_static_vector_; /**< Used to grab the first set of baro measurements */
 

rosplane/include/estimator_example.hpp

@@ -5,6 +5,7 @@
 
 #include <Eigen/Geometry>
 #include <math.h>
+#include <yaml-cpp/yaml.h>
 
 namespace rosplane
 {
@@ -13,7 +14,7 @@ class estimator_example : public estimator_base
 {
 public:
   estimator_example();
-  estimator_example(float init_lat, float init_long, float init_alt);
+  estimator_example(bool use_params);
 
 private:
   virtual void estimate(const input_s & input, output_s & output);

rosplane/launch/rosplane.launch.py

@@ -1,69 +1,70 @@
 import os
 import sys
+import launch.actions
 from launch import LaunchDescription
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 
+
 def generate_launch_description():
     # Create the package directory
     rosplane_dir = get_package_share_directory('rosplane')
 
     # Determine the appropriate control scheme.
     control_type = "default"
     aircraft = "skyhunter" # Default aircraft 
-    init_lat = "0.0" # init lat if seeding the estimator (typically not done)
-    init_long = "0.0"
-    init_alt = "0.0"
-    init_baro_alt = "0.0"
+    use_params = 'false'
 
     for arg in sys.argv:
         if arg.startswith("control_type:="):
             control_type = arg.split(":=")[1]
-        
+
         if arg.startswith("aircraft:="):
             aircraft = arg.split(":=")[1]
-
-        if arg.startswith("init_lat:="):
-            init_lat = float(arg.split(":=")[1])
-            assert isinstance(init_lat, float)
-            init_lat = str(init_lat)
 
-        if arg.startswith("init_long:="):
-            init_long = float(arg.split(":=")[1])
-            assert isinstance(init_long, float)
-            init_long = str(init_long)
+        if arg.startswith("seed_estimator:="):
+            use_params = arg.split(":=")[1].lower()
 
-        if arg.startswith("init_alt:="):
-            init_alt = float(arg.split(":=")[1])
-            assert isinstance(init_alt, float)
-            init_alt = str(init_alt)
-
     autopilot_params = os.path.join(
         rosplane_dir,
         'params',
         aircraft + '_autopilot_params.yaml'
     )
 
     return LaunchDescription([
+        launch.actions.DeclareLaunchArgument(
+            'command_publisher_remap',
+            default_value='/command',
+        ),
         Node(
             package='rosplane',
             executable='rosplane_controller',
             name='autopilot',
-            parameters = [autopilot_params],
-            output = 'screen',
-            arguments = [control_type]
+            parameters=[autopilot_params],
+            output='screen',
+            arguments=[control_type],
+            remappings=[
+                ('/command', launch.substitutions.LaunchConfiguration('command_publisher_remap'))
+            ]
+        ),
+        launch.actions.DeclareLaunchArgument(
+            'controller_command_publisher_remap',
+            default_value='/controller_command',
         ),
         Node(
             package='rosplane',
             executable='rosplane_path_follower',
             name='path_follower',
-            parameters = [autopilot_params],
+            parameters=[autopilot_params],
+            remappings=[
+                ('/controller_command', launch.substitutions.LaunchConfiguration('controller_command_publisher_remap'))
+            ]
         ),
         Node(
             package='rosplane',
             executable='rosplane_path_manager',
             name='path_manager',
-            parameters = [autopilot_params],
+            parameters=[autopilot_params],
         ),
         Node(
             package='rosplane',
@@ -76,6 +77,6 @@ def generate_launch_description():
             name='estimator',
             output='screen',
             parameters = [autopilot_params],
-            arguments = [init_lat, init_long, init_alt, init_baro_alt]
+            arguments = [use_params]
         )
     ])

rosplane/params/anaconda_autopilot_params.yaml

@@ -1,4 +1,4 @@
-autopilot: # node name.
+autopilot:
   ros__parameters:
     alt_hz: 10.0
     alt_toz: 5.0
@@ -31,7 +31,7 @@ autopilot: # node name.
     trim_t: 0.5
     max_e: 0.61
     max_a: 0.60
-    max_r: 0.523  # TODO: Was hardcoded as 1.0 in code...
+    max_r: 0.523
     max_t: 1.0
     pwm_rad_e: 1.0
     pwm_rad_a: 1.0
@@ -41,23 +41,20 @@ autopilot: # node name.
     gravity: 9.8
     max_roll: 35.0
     controller_output_frequency: 100.0
-
 path_manager:
   ros__parameters:
     R_min: 100.0
     orbit_last: False
     default_altitude: 50.0
     default_airspeed: 25.0
     current_path_pub_frequency: 100.0
-
 path_follower:
   ros__parameters:
     controller_commands_pub_frequency: 10.0
     chi_infty: 0.5
     k_orbit: 4.0
     k_path: 0.05
     gravity: 9.81
-
 estimator:
   ros__parameters:
     rho: 1.225
@@ -71,4 +68,9 @@ estimator:
     lpf_a1: 8.0
     gps_n_lim: 10000.
     gps_e_lim: 10000.
-    frequency: 100
+    frequency: 100
+    # These will be overridden on each boot the workspace is symlink installed.
+    baro_calibration_val: 0.0
+    init_lat: 0.0
+    init_lon: 0.0
+    init_alt: 0.0

rosplane/src/controller_base.cpp

@@ -18,7 +18,7 @@ controller_base::controller_base()
 
   // Advertise subscribed topics and set bound callbacks.
   controller_commands_sub_ = this->create_subscription<rosplane_msgs::msg::ControllerCommands>(
-    "controller_commands", 10, std::bind(&controller_base::controller_commands_callback, this, _1));
+    "controller_command", 10, std::bind(&controller_base::controller_commands_callback, this, _1));
   vehicle_state_sub_ = this->create_subscription<rosplane_msgs::msg::State>(
     "estimated_state", 10, std::bind(&controller_base::vehicle_state_callback, this, _1));