Merge branch 'main' into rename-classes-enums-structs

README.md

@@ -30,13 +30,13 @@ To run ROSplane in simulation do the following:
 1. Launch Gazebo and firmware simulation with `ros2 launch rosflight_sim fixedwing.launch.py`
 
 2. Next, run the rosflight_io node configured for SIL with `ros2 run rosflight_io rosflight_io --ros-args -p udp:=true`
-3. Update the firmware parameters for fixedwing flight with `ros2 service call /param_load_from_file rosflight_msgs/srv/ParamFile "filename: rosflight2/rosflight_utils/params/fixedwing_param.yml"` if this is the first time launching.
+3. Update the firmware parameters for fixedwing flight with `ros2 service call /param_load_from_file rosflight_msgs/srv/ParamFile "filename: rosflight/rosflight_sim/params/fixedwing_firmware.yaml"` if this is the first time launching.
 4. Write the new parameters to memory for convenience with `ros2 service call /param_write std_srvs/srv/Trigger`
    - Note: the firmware will time out and not allow takeoff after 100 seconds, so you may need to redo steps one and two.
 5. Calibrate the IMU to allow the airplane to be armed with `ros2 service call /calibrate_imu std_srvs/srv/Trigger`
 6. Then launch ROSplane with `ros2 launch rosplane_sim sim.launch.py`, this will run with the default controller. To use the total energy controller add the argument `control_type:=total_energy`.
 7. Connect a controller with `ros2 run rosflight_utils rc_joy.py --ros-args --remap /RC:=/fixedwing/RC` or if you do not want to use a controller, run `python3 rc_sim.py --ros-args --remap RC:=/fixedwing/RC` while inside `rosflight2/rosflight_utils/src`.
-8. Finally, arm the aircraft with channel 4 and then disable throttle and attitude override with channel 5. If not using a controller use `ros2 service call /arm std_srvs/srv/Trigger` and then disable RC override with `ros2 service call /disable_override std_srvs/srv/Trigger`.
+8. Finally, arm the aircraft with channel 4 and then disable throttle and attitude override with channel 5. If not using a controller use `ros2 service call /toggle_arm std_srvs/srv/Trigger` and then disable RC override with `ros2 service call /toggle_overide std_srvs/srv/Trigger`.
 9. The aircraft should now be airborne!
 
 ## Running ROSplane on hardware.

rosplane/src/path_manager_base.cpp

@@ -41,6 +41,8 @@ PathManagerBase::PathManagerBase()
 void PathManagerBase::declare_parameters() {
   params_.declare_double("R_min", 50.0);
   params_.declare_double("current_path_pub_frequency", 100.0);
+  params_.declare_double("default_altitude", 50.0);
+  params_.declare_double("default_airspeed", 15.0);
 }
 
 void PathManagerBase::set_timer() {
@@ -91,6 +93,7 @@ void PathManagerBase::vehicle_state_callback(const rosplane_msgs::msg::State & m
 void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg)
 {
   double R_min = params_.get_double("R_min");
+  double default_altitude = params_.get_double("default_altitude");
   orbit_dir_ = 0;
 
   // If the message contains "clear_wp_list", then clear all waypoints and do nothing else
@@ -109,7 +112,14 @@ void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint &
 
     temp_waypoint.w[0] = vehicle_state_.position[0];
     temp_waypoint.w[1] = vehicle_state_.position[1];
-    temp_waypoint.w[2] = vehicle_state_.position[2];
+
+    if (vehicle_state_.position[2] < -default_altitude) {
+
+      temp_waypoint.w[2] = vehicle_state_.position[2];
+    }
+    else {
+      temp_waypoint.w[2] = -default_altitude;
+    }
 
     temp_waypoint.chi_d = 0.0; // Doesn't matter, it is never used.
     temp_waypoint.use_chi = false;

rosplane/src/path_manager_example.cpp

@@ -38,6 +38,12 @@ void PathManagerExample::manage(const Input & input, Output & output)
       RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No waypoints received, orbiting origin at " << default_altitude << " meters.");
       output.flag = false; // Indicate that the path is an orbit.
       output.va_d = default_airspeed; // Set to the default_airspeed.
+      output.q[0] = 0.0f; // initialize the parameters to have a value.
+      output.q[1] = 0.0f;
+      output.q[2] = 0.0f;
+      output.r[0] = 0.0f; // initialize the parameters to have a value.
+      output.r[1] = 0.0f;
+      output.r[2] = 0.0f;
       output.c[0] = 0.0f; // Direcct the center of the orbit to the origin at the default default_altitude.
       output.c[1] = 0.0f;
       output.c[2] = -default_altitude;
@@ -50,6 +56,12 @@ void PathManagerExample::manage(const Input & input, Output & output)
     // If only a single waypoint is given, orbit it.
     output.flag = false;
     output.va_d = waypoints_[0].va_d;
+    output.q[0] = 0.0f; // initialize the parameters to have a value.
+    output.q[1] = 0.0f;
+    output.q[2] = 0.0f;
+    output.r[0] = 0.0f; // initialize the parameters to have a value.
+    output.r[1] = 0.0f;
+    output.r[2] = 0.0f;
     output.c[0] = waypoints_[0].w[0];
     output.c[1] = waypoints_[0].w[1];
     output.c[2] = waypoints_[0].w[2];
@@ -143,6 +155,7 @@ void PathManagerExample::manage_fillet(const Input & input,
 
   if (orbit_last && idx_a_ == num_waypoints_ - 1)
   {
+    // TODO: check to see if this is the correct behavior.
     return;
   }
 
@@ -561,8 +574,6 @@ void PathManagerExample::dubins_parameters(const Waypoint start_node, const Wayp
 void PathManagerExample::declare_parameters()
 {
   params_.declare_bool("orbit_last", false);
-  params_.declare_double("default_altitude", 50.0);
-  params_.declare_double("default_airspeed", 15.0);
 }
 
 int PathManagerExample::orbit_direction(float pn, float pe, float chi, float c_n, float c_e)

rosplane_tuning/CMakeLists.txt

@@ -57,29 +57,6 @@ install(TARGETS
 
 #### END OF EXECUTABLES ###
 
-#### RQT ####
-
-ament_python_install_package(rqt_tuning_gui
-  PACKAGE_DIR src/rqt_tuning_gui
-)
-
-install(FILES
-  plugin.xml
-  DESTINATION share/${PROJECT_NAME}
-)
-
-install(DIRECTORY
-  resource
-  DESTINATION share/${PROJECT_NAME}
-)
-
-install(PROGRAMS
-  scripts/rqt_tuning_gui
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-#### END OF RQT ####
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

rosplane_tuning/package.xml

@@ -23,7 +23,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
-    <rqt_gui plugin="${prefix}/plugin.xml" />
   </export>
 
 </package>