Upstream Merge 02/11/2020

.github/workflows/catkin_build_test.yml

@@ -0,0 +1,38 @@
+
+name: Catkin Build Test
+on:
+  push:
+    branches:
+    - 'master'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        config:
+          - {rosdistro: 'melodic', container: 'px4io/px4-dev-ros-melodic:2020-11-02'}
+          - {rosdistro: 'noetic', container: 'px4io/px4-dev-ros-noetic:2020-11-02'}
+    container: ${{ matrix.config.container }}
+    steps:
+    - uses: actions/checkout@v1
+      with:
+        token: ${{ secrets.ACCESS_TOKEN }}
+        github-token: ${{ secrets.GITHUB_TOKEN }}
+    - name: release_build_test
+      working-directory: 
+      run: |
+        mkdir -p $HOME/catkin_ws/src;
+        cd $HOME/catkin_ws
+        catkin init
+        catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}"
+        catkin config --merge-devel
+        cd $HOME/catkin_ws/src
+        ln -s $GITHUB_WORKSPACE
+        cd $HOME/catkin_ws
+        catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False
+        catkin build -j$(nproc) -l$(nproc) jsbsim_bridge

.github/workflows/check_style.yml

@@ -0,0 +1,29 @@
+name: Style Checks
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+    branches:
+      - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        container:
+        - 'px4io/px4-dev-simulation-focal:2020-09-14'
+    container: ${{ matrix.container }}
+    steps:
+    - uses: actions/checkout@v1
+    - name: submodule update
+      run: git submodule update --init --recursive
+    - name: Install clang-format
+      run: apt update && apt install -y clang-format-6.0
+    - name: Check Code format
+      working-directory: scripts
+      run: |
+        ./check_code_format.sh ..

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "models/Rascal"]
 	path = models/Rascal
-	url = https://github.com/ThunderFly-aerospace/FlightGear-Rascal.git
+	url = https://github.com/Auterion/FlightGear-Rascal.git
 [submodule "models/ATI-Resolution"]
 	path = models/ATI-Resolution
 	url = https://github.com/FGMEMBERS/ATI-Resolution.git

CMakeLists.txt

@@ -83,3 +83,53 @@ target_include_directories(jsbsim_bridge
 				PUBLIC ${MAVLINK_INCLUDE_DIRS} ${JSBSIM_INCLUDE_DIR} ${Boost_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS})
 
 target_link_libraries(jsbsim_bridge ${JSBSIM_LIBRARY} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_TIMER_LIBRARY_RELEASE} ${EIGEN_LIBRARIES} ${TinyXML_LIBRARIES})
+
+# see if catkin was invoked to build this
+if (CATKIN_DEVEL_PREFIX)
+	message(STATUS "catkin ENABLED")
+	find_package(catkin REQUIRED)
+	if (catkin_FOUND)
+		catkin_package()
+	else()
+		message(FATAL_ERROR "catkin not found")
+	endif()
+
+	find_package(catkin REQUIRED COMPONENTS
+			roscpp
+			rospy
+			mavlink
+		)
+
+	catkin_package(
+		INCLUDE_DIRS include
+		LIBRARIES geometric_controller
+		CATKIN_DEPENDS roscpp rospy std_msgs
+	)
+
+  add_executable(jsbsim_bridge_node
+	src/jsbsim_bridge_node.cpp
+	src/jsbsim_bridge_ros.cpp
+	src/configuration_parser.cpp
+	src/jsbsim_bridge.cpp
+	src/geo_mag_declination.cpp
+	src/mavlink_interface.cpp
+	src/actuator_plugin.cpp
+	src/sensor_plugin.cpp
+	src/sensor_airspeed_plugin.cpp
+	src/sensor_baro_plugin.cpp
+	src/sensor_imu_plugin.cpp
+	src/sensor_gps_plugin.cpp
+	src/sensor_mag_plugin.cpp
+	)
+
+  target_include_directories(jsbsim_bridge_node
+    BEFORE
+	PUBLIC ${catkin_INCLUDE_DIRS} ${MAVLINK_INCLUDE_DIRS} ${JSBSIM_INCLUDE_DIR} ${Boost_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS}
+	)
+
+	target_link_libraries(jsbsim_bridge_node ${catkin_LIBRARIES} ${JSBSIM_LIBRARY} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_TIMER_LIBRARY_RELEASE} ${EIGEN_LIBRARIES} ${TinyXML_LIBRARIES}
+	)
+
+  else()
+  message(STATUS "catkin DISABLED")
+endif()

configs/quadrotor_x.xml

@@ -4,6 +4,12 @@
     </mavlink_interface>
     <sensors>
         <imu>
+            <jsb_acc_x>px4/acc-x</jsb_acc_x>
+            <jsb_acc_y>px4/acc-y</jsb_acc_y>
+            <jsb_acc_z>px4/acc-z</jsb_acc_z>
+            <jsb_gyro_x>px4/gyro-x</jsb_gyro_x>
+            <jsb_gyro_y>px4/gyro-y</jsb_gyro_y>
+            <jsb_gyro_z>px4/gyro-z</jsb_gyro_z>
         </imu>
         <gps>
             <jsb_gps_fix_type>px4/gps-fix-type</jsb_gps_fix_type>
@@ -19,6 +25,8 @@
             <jsb_gps_satellites>px4/gps-satellites-visible</jsb_gps_satellites>
         </gps>
         <barometer>
+            <jsb_baro_temp>px4/baro-temp</jsb_baro_temp>
+            <jsb_baro_air_pressure>px4/baro-abs-pressure</jsb_baro_air_pressure>
         </barometer>
         <magnetometer>
         </magnetometer>

include/common.h

@@ -47,6 +47,7 @@
 inline double ftToM(double ft) { return 0.3048 * ft; }
 inline double rankineToCelsius(double temp) { return (temp - 491.67) * 5 / 9; }
 inline double psfToBar(double pressure) { return 0.000478802588 * pressure; }
+inline double psfToMbar(double pressure) { return 0.478802588 * pressure; }
 
 inline double wrap_pi(double x) {
   while (x > M_PI) {
@@ -60,6 +61,13 @@ inline double wrap_pi(double x) {
   return x;
 }
 
+inline double wrap_pi_deg(double x) {
+  while (x < 0) {
+    x += 360;
+  }
+  return x;
+}
+
 inline bool CheckConfigElement(const TiXmlElement &config, std::string param) {
   const TiXmlElement *e = config.FirstChildElement(param);
   return e != nullptr;

include/configuration_parser.h

@@ -42,27 +42,27 @@
 
 #include "common.h"
 
-#include <memory>
 #include <tinyxml.h>
 #include <Eigen/Eigen>
+#include <memory>
 
-enum class ArgResult {
-  Success, Help, Error
-};
+enum class ArgResult { Success, Help, Error };
 
 class ConfigurationParser {
  public:
-
   ConfigurationParser() = default;
   ~ConfigurationParser() = default;
   bool ParseEnvironmentVariables();
   bool ParseConfigFile(const std::string& path);
   ArgResult ParseArgV(int argc, char* const argv[]);
-  inline bool isHeadless() { return _headless; }
-  inline std::shared_ptr<TiXmlHandle> XmlHandle() { return _config; }
-  inline std::string getInitScriptPath() { return _init_script_path; }
-  inline std::string getModelName() { return _model_name; }
-  static void PrintHelpMessage(char *argv[]);
+  bool isHeadless() { return _headless; }
+  std::shared_ptr<TiXmlHandle> XmlHandle() { return _config; }
+  std::string getInitScriptPath() { return _init_script_path; }
+  std::string getModelName() { return _model_name; }
+  int getRealtimeFactor() { return _realtime_factor; }
+  void setHeadless(bool headless) { _headless = headless; }
+  void setInitScriptPath(std::string path) { _init_script_path = path; }
+  static void PrintHelpMessage(char* argv[]);
 
  private:
   TiXmlDocument _doc;
@@ -71,4 +71,5 @@ class ConfigurationParser {
   bool _headless{false};
   std::string _init_script_path;
   std::string _model_name;
+  float _realtime_factor{1.0};
 };

include/jsbsim_bridge.h

@@ -80,8 +80,7 @@ class JSBSimBridge {
   std::unique_ptr<SensorAirspeedPlugin> _airspeed_sensor;
   std::unique_ptr<ActuatorPlugin> _actuators;
 
-  std::chrono::time_point<std::chrono::system_clock> _last_step_time;
-  double _dt;
-  bool _realtime;
-  bool _result;
+  double _dt{0.004};
+  double _realtime_factor{1.0};
+  bool _result{true};
 };

include/jsbsim_bridge_ros.h

@@ -0,0 +1,78 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 Auterion AG. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ *
+ * @author Jaeyoung Lim <jaeyoung@auterion.com>
+ *
+ */
+
+#ifndef JSBSIM_BRIDGE_ROS_H
+#define JSBSIM_BRIDGE_ROS_H
+
+#include "jsbsim_bridge.h"
+
+#include <ros/ros.h>
+
+#include <stdio.h>
+#include <cstdlib>
+#include <sstream>
+#include <string>
+
+#include <Eigen/Dense>
+
+using namespace std;
+using namespace Eigen;
+
+class JSBSimBridgeRos {
+ public:
+  JSBSimBridgeRos(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);
+  virtual ~JSBSimBridgeRos();
+
+ private:
+  void simloopCallback(const ros::TimerEvent& event);
+  void statusloopCallback(const ros::TimerEvent& event);
+
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+  ros::Timer simloop_timer_, statusloop_timer_;
+
+  JSBSim::FGFDMExec* fdmexec_;
+  ConfigurationParser config_;
+  std::unique_ptr<JSBSimBridge> jsbsim_bridge_;
+
+  std::string path;
+  std::string script_path;
+};
+#endif

include/sensor_airspeed_plugin.h

@@ -52,9 +52,8 @@ class SensorAirspeedPlugin : public SensorPlugin {
   SensorData::Airspeed getData();
 
  private:
-  double getAirspeed();
-  double getAirTemperature();
-
+  double getDiffPressure();
+  std::string _jsb_diff_pressure = "aero/qbar-psf";
   std::normal_distribution<double> standard_normal_distribution_;
   double _diff_pressure_stddev{0.01};
 };

include/sensor_baro_plugin.h

@@ -56,10 +56,24 @@ class SensorBaroPlugin : public SensorPlugin {
   float getPressureAltitude();
   float getAirPressure();
 
+  double _abs_pressure;
+
+  void addNoise(double abs_pressure, const double dt);
+
   std::normal_distribution<double> _standard_normal_distribution;
 
   // state variables for baro pressure sensor random noise generator
   double _baro_rnd_y2{0.};
+  double _baro_mbar_rms_noise{1.};
   bool _baro_rnd_use_last{false};
-  double _baro_drift_pa{0.};
+  double _baro_drift_mbar{0.};
+  double _baro_drift_mbar_per_sec{0.};
+
+  // Variable for temperature sensor random noise
+  double _temperature_stddev{0.02};
+
+  // JSBSim default variables
+  std::string _jsb_baro_temp = "atmosphere/T-R";
+  std::string _jsb_baro_pressure_alt = "atmosphere/pressure-altitude";
+  std::string _jsb_baro_air_pressure = "atmosphere/P-psf";
 };

include/sensor_gps_plugin.h

@@ -53,15 +53,15 @@ class SensorGpsPlugin : public SensorPlugin {
 
  private:
   SensorData::Gps getGpsFromJSBSim();
-  std::string jsb_gps_fix_type = "none";
-  std::string jsb_gps_lat = "position/lat-geod-deg";
-  std::string jsb_gps_lon = "position/long-gc-deg";
-  std::string jsb_gps_alt = "position/h-sl-meters";
-  std::string jsb_gps_eph = "none";
-  std::string jsb_gps_epv = "none";
-  std::string jsb_gps_v_north = "velocities/v-north-fps";
-  std::string jsb_gps_v_east = "velocities/v-east-fps";
-  std::string jsb_gps_v_down = "velocities/v-down-fps";
-  std::string jsb_gps_velocity = "velocities/ned-velocity-mag-fps";
-  std::string jsb_gps_satellites = "none";
+  std::string _jsb_gps_fix_type = "none";
+  std::string _jsb_gps_lat = "position/lat-geod-deg";
+  std::string _jsb_gps_lon = "position/long-gc-deg";
+  std::string _jsb_gps_alt = "position/h-sl-meters";
+  std::string _jsb_gps_eph = "none";
+  std::string _jsb_gps_epv = "none";
+  std::string _jsb_gps_v_north = "velocities/v-north-fps";
+  std::string _jsb_gps_v_east = "velocities/v-east-fps";
+  std::string _jsb_gps_v_down = "velocities/v-down-fps";
+  std::string _jsb_gps_velocity = "velocities/ned-velocity-mag-fps";
+  std::string _jsb_gps_satellites = "none";
 };

include/sensor_imu_plugin.h

@@ -95,4 +95,17 @@ class SensorImuPlugin : public SensorPlugin {
   Eigen::Vector3d _accelerometer_turn_on_bias{Eigen::Vector3d::Zero()};
 
   std::normal_distribution<double> _standard_normal_distribution;
+
+  /** Accelerations are affected by JSBSim airframe configuration <location name="EYEPOINT">
+   * ensure you have set the eyepoint location as to where you expect accelerometer measurements
+   * or more appropriately, use the px4_default_imu_sensor.xml configuration.
+   */
+  std::string _jsb_acc_x = "accelerations/a-pilot-x-ft_sec2";
+  std::string _jsb_acc_y = "accelerations/a-pilot-y-ft_sec2";
+  std::string _jsb_acc_z = "accelerations/a-pilot-z-ft_sec2";
+
+  // PX4 requires axis angular acceleration vs. body frame acceleration.
+  std::string _jsb_gyro_x = "velocities/p-rad_sec";
+  std::string _jsb_gyro_y = "velocities/q-rad_sec";
+  std::string _jsb_gyro_z = "velocities/r-rad_sec";
 };