Merge pull request #161 from FS-Online/ground-speed-sensor

SijmenHuizenga · web-flow · commit 00fa0025fa38 · 2020-07-24T00:29:51.000+02:00
Ground speed sensor
diff --git a/AirSim/AirLib/AirLib.vcxproj b/AirSim/AirLib/AirLib.vcxproj
@@ -94,6 +94,8 @@
     <ClInclude Include="include\sensors\imu\ImuBase.hpp" />
     <ClInclude Include="include\sensors\imu\ImuSimple.hpp" />
     <ClInclude Include="include\sensors\imu\ImuSimpleParams.hpp" />
+    <ClInclude Include="include\sensors\gss\GSSSimple.hpp" />
+
     <ClInclude Include="include\sensors\SensorBase.hpp" />
     <ClInclude Include="include\sensors\SensorCollection.hpp" />
   </ItemGroup>
diff --git a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp
@@ -655,6 +655,32 @@ class RpcLibAdapatorsBase {
         }
     };
 
+    struct GSSData {
+
+        msr::airlib::TTimePoint time_stamp;
+        Vector3r linear_velocity;
+
+        MSGPACK_DEFINE_MAP(time_stamp, linear_velocity);
+
+        GSSData()
+        {}
+
+        GSSData(const msr::airlib::GSSSimple::Output& o)
+        {
+            time_stamp = o.time_stamp;
+            linear_velocity = o.linear_velocity;
+        }
+
+        msr::airlib::GSSSimple::Output to() const
+        {
+            msr::airlib::GSSSimple::Output s;
+            s.linear_velocity = linear_velocity.to();
+            s.time_stamp = time_stamp;
+ 
+            return s;
+        }
+    };
+
 
 };
 
diff --git a/AirSim/AirLib/include/api/RpcLibClientBase.hpp b/AirSim/AirLib/include/api/RpcLibClientBase.hpp
@@ -77,6 +77,7 @@ class RpcLibClientBase {
     msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = "") const;
     msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const;
     msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const;
+    msr::airlib::GSSSimple::Output getGroundSpeedSensorData(const std::string& vehicle_name = "") const;
 
     // sensor omniscient APIs
     vector<int> simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;
diff --git a/AirSim/AirLib/include/api/VehicleApiBase.hpp b/AirSim/AirLib/include/api/VehicleApiBase.hpp
@@ -14,6 +14,7 @@
 #include "sensors/lidar/LidarBase.hpp"
 #include "sensors/imu/ImuBase.hpp"
 #include "sensors/distance/DistanceBase.hpp"
+#include "sensors/gss/GSSSimple.hpp"
 #include "sensors/gps/GpsBase.hpp"
 #include <exception>
 #include <string>
@@ -179,6 +180,21 @@ class VehicleApiBase : public UpdatableObject {
         return distance_sensor->getOutput();
     }
 
+    virtual GSSSimple::Output getGroundSpeedSensorData() const 
+    {
+        uint count_gss = getSensors().size(SensorBase::SensorType::GSS);
+        if(count_gss == 0) {
+            throw VehicleControllerException(Utils::stringf("No ground speed sensor on vehicle"));
+        }
+        const GSSSimple* gss = static_cast<const GSSSimple*>(getSensors().getByType(SensorBase::SensorType::GSS, 0));
+        if (gss == nullptr)
+        {
+            throw VehicleControllerException(Utils::stringf("Ground speed sensor nullptr"));
+        }
+
+        return gss->getOutput();
+    }
+
     virtual ~VehicleApiBase() = default;
 
     //exceptions
diff --git a/AirSim/AirLib/include/common/AirSimSettings.hpp b/AirSim/AirLib/include/common/AirSimSettings.hpp
@@ -1019,8 +1019,13 @@ struct AirSimSettings {
         case SensorBase::SensorType::Lidar:
             sensor_setting = std::unique_ptr<SensorSetting>(new LidarSetting());
             break;
+        case SensorBase::SensorType::GSS:
+            // at this moment the codebase requires every sensor to have settings.
+            // However, gss does not have settings... so we just place ImuSettings in here for now.
+            sensor_setting = std::unique_ptr<SensorSetting>(new ImuSetting());
+            break;
         default:
-            throw std::invalid_argument("Unexpected sensor type");
+            throw std::invalid_argument("Unexpected sensor type (B)");
         }
 
         sensor_setting->sensor_type = sensor_type;
@@ -1047,8 +1052,10 @@ struct AirSimSettings {
         case SensorBase::SensorType::Lidar:
             initializeLidarSetting(*static_cast<LidarSetting*>(sensor_setting), settings_json);
             break;
+        case SensorBase::SensorType::GSS:
+            break;
         default:
-            throw std::invalid_argument("Unexpected sensor type");
+            throw std::invalid_argument("Unexpected sensor type (A)");
         }
     }
 
diff --git a/AirSim/AirLib/include/sensors/SensorBase.hpp b/AirSim/AirLib/include/sensors/SensorBase.hpp
@@ -23,7 +23,8 @@ class SensorBase : public UpdatableObject  {
         Imu = 2,
         Gps = 3,
         Distance = 5,
-        Lidar = 6
+        Lidar = 6,
+        GSS = 7,
     };
 
     SensorBase(const std::string& sensor_name = "")
diff --git a/AirSim/AirLib/include/sensors/SensorFactory.hpp b/AirSim/AirLib/include/sensors/SensorFactory.hpp
@@ -25,8 +25,10 @@ class SensorFactory {
             return std::unique_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
         case SensorBase::SensorType::Gps:
             return std::unique_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
+        case SensorBase::SensorType::GSS:
+            return std::unique_ptr<GSSSimple>(new GSSSimple());
         default:
-            throw new std::invalid_argument("Unexpected sensor type");
+            throw new std::invalid_argument("Unexpected sensor type (C)");
         }
     }
 
diff --git a/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp b/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp
@@ -0,0 +1,51 @@
+#ifndef msr_airlib_GSSSimple_hpp
+#define msr_airlib_GSSSimple_hpp
+
+
+#include "sensors/SensorBase.hpp"
+
+
+namespace msr { namespace airlib {
+
+class GSSSimple  : public SensorBase {
+public:
+    // Ground Speed Sensor
+    GSSSimple(const std::string& sensor_name = "")
+        : SensorBase(sensor_name)
+    {}
+
+public:
+    struct Output {
+        TTimePoint time_stamp;
+        Vector3r linear_velocity;
+    };
+
+public:
+    virtual void update() override
+    {
+        Output output;
+        const GroundTruth& ground_truth = getGroundTruth();
+
+        output.time_stamp = clock()->nowNanos();
+        output.linear_velocity = Vector3r(std::sqrt(std::pow(ground_truth.kinematics->twist.linear.x(), 2) + std::pow(ground_truth.kinematics->twist.linear.y(), 2)), 0, ground_truth.kinematics->twist.linear.z());
+
+        output_ = output;
+    }
+    const Output& getOutput() const
+    {
+        return output_;
+    }
+
+    virtual ~GSSSimple() = default;
+
+    virtual void resetImplementation() override {
+
+    }
+
+private:
+    Output output_;
+};
+
+
+}}
+#endif 
diff --git a/AirSim/AirLib/src/api/RpcLibClientBase.cpp b/AirSim/AirLib/src/api/RpcLibClientBase.cpp
@@ -175,6 +175,11 @@ msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const
     return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as<RpcLibAdapatorsBase::DistanceSensorData>().to();
 }
 
+msr::airlib::GSSSimple::Output RpcLibClientBase::getGroundSpeedSensorData(const std::string& vehicle_name) const
+{
+    return pimpl_->client.call("getGroundSpeedSensorData", vehicle_name).as<RpcLibAdapatorsBase::GSSData>().to();
+}
+
 vector<int> RpcLibClientBase::simGetLidarSegmentation(const std::string& lidar_name, const std::string& vehicle_name) const
 {
     return pimpl_->client.call("simGetLidarSegmentation", lidar_name, vehicle_name).as<vector<int>>();
diff --git a/AirSim/AirLib/src/api/RpcLibServerBase.cpp b/AirSim/AirLib/src/api/RpcLibServerBase.cpp
@@ -213,6 +213,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::DistanceSensorData(distance_sensor_data);
     });
 
+    pimpl_->server.bind("getGroundSpeedSensorData", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::GSSData {
+        const auto& gss_data = getVehicleApi(vehicle_name)->getGroundSpeedSensorData();
+        return RpcLibAdapatorsBase::GSSData(gss_data);
+    });
+
     pimpl_->server.bind("simGetCameraInfo", [&](const std::string& camera_name, const std::string& vehicle_name) -> RpcLibAdapatorsBase::CameraInfo {
         const auto& camera_info = getVehicleSimApi(vehicle_name)->getCameraInfo(camera_name);
         return RpcLibAdapatorsBase::CameraInfo(camera_info);
diff --git a/UE4Project/Plugins/AirSim/Source/AirLib.vcxproj b/UE4Project/Plugins/AirSim/Source/AirLib.vcxproj
@@ -94,6 +94,7 @@
     <ClInclude Include="include\sensors\imu\ImuBase.hpp" />
     <ClInclude Include="include\sensors\imu\ImuSimple.hpp" />
     <ClInclude Include="include\sensors\imu\ImuSimpleParams.hpp" />
+    <ClInclude Include="include\sensors\gss\GSSSimple.hpp" />
     <ClInclude Include="include\sensors\SensorBase.hpp" />
     <ClInclude Include="include\sensors\SensorCollection.hpp" />
   </ItemGroup>
diff --git a/docs/ground-speed-sensor.md b/docs/ground-speed-sensor.md
@@ -0,0 +1,31 @@
+# Ground Speed Sensor (GSS)
+
+The ground speed sensor is modeled around the Kistler ground speed (like the Kistler Correvit SFII).
+
+Velocity information is captured in the frame of the car in the NED frame.
+
+At this moment no extra noise is added to the sensordata since the kistler 250hz data averaged into the 100hz is so close to ground truth that adding noise would be unrealistic.
+
+## Ros
+When using the ros bridge, ground speed sensordata will be published on `/fsds/gss` with the `geometry_msgs/TwistStamped` message type.
+
+Appart from the header fields, only `x` and `z` of the `twist.linear` are populated. 
+All other values will be 0.
+
+```
+header:
+  seq: 5747
+  stamp:
+    secs: 1595325426
+    nsecs: 617730500
+  frame_id: "fsds/FSCar"
+twist:
+  linear:
+    x: 4.80838251114
+    y: -0.0
+    z: -0.0214105024934
+  angular:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+```
diff --git a/docs/ros-bridge.md b/docs/ros-bridge.md
@@ -22,6 +22,7 @@ The fsds_ros_bridge.launch launches the following nodes:
 |---|---|---|---|
 | `/fsds/gps` | This the current GPS coordinates of the drone in airsim. Read all about the gps simulation model [here](gps.md). Data is in the `fsds/FSCar` frame. | [sensor_msgs/NavSatFix](https://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html) | 10 |
 | `/fsds/imu` | Velocity, orientation and acceleration information. Read all about the IMU model [here](imu.md). Data is in the `fsds/FSCar` (enu) frame. | [sensor_msgs/Imu](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html)  | 250 |
+| `/fsds/gss` | Ground speed sensor provide linear velocity of the vehicle (`fsds/FSCar`). Velocity is m/s. | [geometry_msgs/TwistStamped](https://docs.ros.org/api/nav_msgs/html/msg/Odometry.html)  | 100 |
 | `/fsds/testing_only/odom` | Ground truth car position and orientation in ENU frame about the CoG of the car (`fsds/FSCar`).  The units are `m` for distance. The orientation are expressed in terms of quaternions. The message is in the `fsds/map` frame. This is a frame that is not (yet) used anywhere else and is just here so you can easely reference it if needed. *THIS WILL NOT BE STREAMED DURING COMPETITION.* | [nav_msgs/Odometry](https://docs.ros.org/api/nav_msgs/html/msg/Odometry.html)  | 250 |
 | `/fsds/testing_only/track` | Ground truth cone position and color with respect to the starting location of the car in ENU. Currently this only publishes the *initial position* of cones that are part of the track spline. Any cones placed manually in the world are not published here. Additionally, the track is published once and the message is latched (meaning it is always available for a newly created subscriber). *THIS WILL NOT BE STREAMED DURING COMPETITION.* | [fs_msgs/Track](https://github.com/FS-Online/fs_msgs/blob/master/msg/Track.msg)  | Latched |
 | `/fsds/camera/CAMERA_NAME` | One of this topic type will exist for every camera specified in the `settings.json` file. On this topic, camera frames are published. The format will be bgra8. `CAMERA_NAME` will be replaced by the corresponding in the `Cameras` object in the `settings.json` file. `IMAGE_TYPE` is determand by the `SensorType` field. When choosing 0, it will be 'Scene'. | [sensor_msgs/Image](https://docs.ros.org/api/sensor_msgs/html/msg/Image.html)  | ~18 |
@@ -77,6 +78,11 @@ Transforms to the ground truth are disabled because this would take away the sta
   Timer callback frequency for updating and publishing the imu messages.   
   This value must be equal or higher to the minimual sample rate of the sensor configured in the settings.json
 
+- `/fsds/ros_bridge/update_gss_every_n_sec` [double]   
+  Set in: `$(fsds_ros_bridge)/launch/fsds_ros_bridge.launch`   
+  Default: 0.01 seconds (100hz).   
+  Timer callback frequency for updating and publishing the ground speed sensor messages.
+
 - `/fsds/ros_bridge/update_odom_every_n_sec` [double]   
   Set in: `$(fsds_ros_bridge)/launch/fsds_ros_bridge.launch`   
   Default: 0.004 seconds (250hz).   
diff --git a/mkdocs.yml b/mkdocs.yml
@@ -17,6 +17,7 @@ nav:
     - "IMU": "imu.md"
     - "Lidar": "lidar.md"
     - "GPS": "gps.md"
+    - "GSS": "gss.md"
   - "#tech madness":
     - "ROS Bridge": "ros-bridge.md"
     - "ROS Bridge statistics": "statistics.md"
diff --git a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h
@@ -97,13 +97,15 @@ class AirsimROSWrapper
     ros_bridge::Statistics getGpsDataStatistics;
     ros_bridge::Statistics getCarStateStatistics;
     ros_bridge::Statistics getImuStatistics;
+    ros_bridge::Statistics getGSSStatistics;
     std::vector<ros_bridge::Statistics> getLidarDataVecStatistics;
     ros_bridge::Statistics control_cmd_sub_statistics;
     ros_bridge::Statistics global_gps_pub_statistics;
     ros_bridge::Statistics odom_pub_statistics;
     std::vector<ros_bridge::Statistics> cam_pub_vec_statistics;
     std::vector<ros_bridge::Statistics> lidar_pub_vec_statistics;
     ros_bridge::Statistics imu_pub_statistics;
+    ros_bridge::Statistics gss_pub_statistics;
     
     // create std::vector<Statistics*> which I can use to iterate over all these options 
     // and apply common operations such as print, reset
@@ -119,6 +121,7 @@ class AirsimROSWrapper
     void odom_cb(const ros::TimerEvent& event);    // update drone state from airsim_client_ every nth sec
     void gps_timer_cb(const ros::TimerEvent& event);
     void imu_timer_cb(const ros::TimerEvent& event);
+    void gss_timer_cb(const ros::TimerEvent& event);
     void statictf_cb(const ros::TimerEvent& event);
     void car_control_cb(const fs_msgs::ControlCommand::ConstPtr& msg, const std::string& vehicle_name);
     void lidar_timer_cb(const ros::TimerEvent& event, const std::string& camera_name, const int lidar_index);
@@ -193,6 +196,7 @@ class AirsimROSWrapper
     ros::Timer odom_update_timer_;
     ros::Timer gps_update_timer_;
     ros::Timer imu_update_timer_;
+    ros::Timer gss_update_timer_;
     std::vector<ros::Timer> airsim_lidar_update_timers_;
     ros::Timer statistics_timer_;
     ros::Timer go_signal_timer_;
@@ -206,6 +210,7 @@ class AirsimROSWrapper
     ros::Publisher odom_pub;
     ros::Publisher global_gps_pub;
     ros::Publisher imu_pub;
+    ros::Publisher gss_pub;
     ros::Publisher track_pub;
     ros::Publisher go_signal_pub_;
     
diff --git a/ros/src/fsds_ros_bridge/launch/fsds_ros_bridge.launch b/ros/src/fsds_ros_bridge/launch/fsds_ros_bridge.launch
@@ -16,6 +16,7 @@
 			<param name="update_odom_every_n_sec" type="double" value="0.004" />
 			<param name="update_imu_every_n_sec" type="double" value="0.004" /> 
 			<param name="update_gps_every_n_sec" type="double" value="0.1" />
+			<param name="update_gss_every_n_sec" type="double" value="0.01" />
 			<param name="publish_static_tf_every_n_sec" type="double" value="1" />
 			<param name="update_lidar_every_n_sec" type="double" value="0.1" />
 			
diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp
diff --git a/settings.json b/settings.json

Original file line number	Diff line number	Diff line change
`@@ -25,8 +25,10 @@ class SensorFactory {`
`25`	`25`	`return std::unique_ptr<ImuSimple>(new ImuSimple(static_cast<const AirSimSettings::ImuSetting>(sensor_setting)));`
`26`	`26`	`case SensorBase::SensorType::Gps:`
`27`	`27`	`return std::unique_ptr<GpsSimple>(new GpsSimple(static_cast<const AirSimSettings::GpsSetting>(sensor_setting)));`
	`28`	`+ case SensorBase::SensorType::GSS:`
	`29`	`+ return std::unique_ptr<GSSSimple>(new GSSSimple());`
`28`	`30`	`default:`
`29`		`- throw new std::invalid_argument("Unexpected sensor type");`
	`31`	`+ throw new std::invalid_argument("Unexpected sensor type (C)");`
`30`	`32`	`}`
`31`	`33`	`}`
`32`	`34`
Original file line number	Diff line number	Diff line change
`@@ -175,6 +175,11 @@ msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const`
`175`	`175`	`return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as<RpcLibAdapatorsBase::DistanceSensorData>().to();`
`176`	`176`	`}`
`177`	`177`
	`178`	`+msr::airlib::GSSSimple::Output RpcLibClientBase::getGroundSpeedSensorData(const std::string& vehicle_name) const`
	`179`	`+{`
	`180`	`+ return pimpl_->client.call("getGroundSpeedSensorData", vehicle_name).as<RpcLibAdapatorsBase::GSSData>().to();`
	`181`	`+}`
	`182`	`+`
`178`	`183`	`vector<int> RpcLibClientBase::simGetLidarSegmentation(const std::string& lidar_name, const std::string& vehicle_name) const`
`179`	`184`	`{`
`180`	`185`	`return pimpl_->client.call("simGetLidarSegmentation", lidar_name, vehicle_name).as<vector<int>>();`