From e1220da6d8255aa065abd25aa48460195a78eb38 Mon Sep 17 00:00:00 2001 From: sijmenhuizenga Date: Sun, 19 Jul 2020 12:47:13 +0200 Subject: [PATCH 1/5] Added ground speed sensor --- AirSim/AirLib/AirLib.vcxproj | 1 + .../include/api/RpcLibAdapatorsBase.hpp | 29 ++++++++++ .../AirLib/include/api/RpcLibClientBase.hpp | 1 + AirSim/AirLib/include/api/VehicleApiBase.hpp | 16 ++++++ .../AirLib/include/common/AirSimSettings.hpp | 11 +++- AirSim/AirLib/include/sensors/SensorBase.hpp | 3 +- .../AirLib/include/sensors/SensorFactory.hpp | 4 +- .../AirLib/include/sensors/gss/GSSSimple.hpp | 51 +++++++++++++++++ AirSim/AirLib/src/api/RpcLibClientBase.cpp | 5 ++ AirSim/AirLib/src/api/RpcLibServerBase.cpp | 5 ++ .../Plugins/AirSim/Source/AirLib.vcxproj | 1 + docs/ros-bridge.md | 5 ++ .../include/airsim_ros_wrapper.h | 5 ++ .../launch/fsds_ros_bridge.launch | 1 + .../src/airsim_ros_wrapper.cpp | 56 ++++++++++++++++++- settings.json | 4 ++ 16 files changed, 193 insertions(+), 5 deletions(-) create mode 100644 AirSim/AirLib/include/sensors/gss/GSSSimple.hpp diff --git a/AirSim/AirLib/AirLib.vcxproj b/AirSim/AirLib/AirLib.vcxproj index 9181babb..ac9c4d85 100644 --- a/AirSim/AirLib/AirLib.vcxproj +++ b/AirSim/AirLib/AirLib.vcxproj @@ -117,6 +117,7 @@ + diff --git a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp index 3deabeb8..1a31eddd 100644 --- a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -757,6 +757,35 @@ class RpcLibAdapatorsBase { } }; + struct GSSData { + + msr::airlib::TTimePoint time_stamp; + Vector3r linear_velocity; + Vector3r angular_velocity; + + MSGPACK_DEFINE_MAP(time_stamp, linear_velocity, angular_velocity); + + GSSData() + {} + + GSSData(const msr::airlib::GSSSimple::Output& o) + { + time_stamp = o.time_stamp; + linear_velocity = o.twist.linear; + angular_velocity = o.twist.angular; + } + + msr::airlib::GSSSimple::Output to() const + { + msr::airlib::GSSSimple::Output s; + s.twist.linear = linear_velocity.to(); + s.twist.angular = angular_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 index 6dffae62..c0f1e7fc 100644 --- a/AirSim/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirSim/AirLib/include/api/RpcLibClientBase.hpp @@ -82,6 +82,7 @@ class RpcLibClientBase { msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_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 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 index 5d676e43..8071d4f4 100644 --- a/AirSim/AirLib/include/api/VehicleApiBase.hpp +++ b/AirSim/AirLib/include/api/VehicleApiBase.hpp @@ -17,6 +17,7 @@ #include "sensors/barometer/BarometerBase.hpp" #include "sensors/magnetometer/MagnetometerBase.hpp" #include "sensors/distance/DistanceBase.hpp" +#include "sensors/gss/GSSSimple.hpp" #include "sensors/gps/GpsBase.hpp" #include #include @@ -235,6 +236,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(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 index 91e46ecb..039388fb 100644 --- a/AirSim/AirLib/include/common/AirSimSettings.hpp +++ b/AirSim/AirLib/include/common/AirSimSettings.hpp @@ -1196,8 +1196,13 @@ struct AirSimSettings { case SensorBase::SensorType::Lidar: sensor_setting = std::unique_ptr(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(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; @@ -1230,8 +1235,10 @@ struct AirSimSettings { case SensorBase::SensorType::Lidar: initializeLidarSetting(*static_cast(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 index 765cfabc..bf573bd3 100644 --- a/AirSim/AirLib/include/sensors/SensorBase.hpp +++ b/AirSim/AirLib/include/sensors/SensorBase.hpp @@ -26,7 +26,8 @@ class SensorBase : public UpdatableObject { Gps = 3, Magnetometer = 4, 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 index 1fffe864..47fb39a2 100644 --- a/AirSim/AirLib/include/sensors/SensorFactory.hpp +++ b/AirSim/AirLib/include/sensors/SensorFactory.hpp @@ -31,8 +31,10 @@ class SensorFactory { return std::unique_ptr(new GpsSimple(*static_cast(sensor_setting))); case SensorBase::SensorType::Barometer: return std::unique_ptr(new BarometerSimple(*static_cast(sensor_setting))); + case SensorBase::SensorType::GSS: + return std::unique_ptr(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 new file mode 100644 index 00000000..29fe5d15 --- /dev/null +++ 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; + Twist twist; + }; + +public: + virtual void update() override + { + Output output; + const GroundTruth& ground_truth = getGroundTruth(); + + output.time_stamp = clock()->nowNanos(); + output.twist = ground_truth.kinematics->twist; + + 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 index 166027a6..e0d9b675 100644 --- a/AirSim/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirSim/AirLib/src/api/RpcLibClientBase.cpp @@ -185,6 +185,11 @@ msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as().to(); } +msr::airlib::GSSSimple::Output RpcLibClientBase::getGroundSpeedSensorData(const std::string& vehicle_name) const +{ + return pimpl_->client.call("getGroundSpeedSensorData", vehicle_name).as().to(); +} + vector RpcLibClientBase::simGetLidarSegmentation(const std::string& lidar_name, const std::string& vehicle_name) const { return pimpl_->client.call("simGetLidarSegmentation", lidar_name, vehicle_name).as>(); diff --git a/AirSim/AirLib/src/api/RpcLibServerBase.cpp b/AirSim/AirLib/src/api/RpcLibServerBase.cpp index 2cb514e0..41191230 100644 --- a/AirSim/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirSim/AirLib/src/api/RpcLibServerBase.cpp @@ -223,6 +223,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 index 9181babb..ac9c4d85 100644 --- a/UE4Project/Plugins/AirSim/Source/AirLib.vcxproj +++ b/UE4Project/Plugins/AirSim/Source/AirLib.vcxproj @@ -117,6 +117,7 @@ + diff --git a/docs/ros-bridge.md b/docs/ros-bridge.md index 1f44d5be..e2652a70 100644 --- a/docs/ros-bridge.md +++ b/docs/ros-bridge.md @@ -77,6 +77,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/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h index 8b8bc78d..ce82ea17 100644 --- a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h +++ b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h @@ -98,6 +98,7 @@ class AirsimROSWrapper ros_bridge::Statistics getGpsDataStatistics; ros_bridge::Statistics getCarStateStatistics; ros_bridge::Statistics getImuStatistics; + ros_bridge::Statistics getGSSStatistics; std::vector getLidarDataVecStatistics; ros_bridge::Statistics control_cmd_sub_statistics; ros_bridge::Statistics global_gps_pub_statistics; @@ -105,6 +106,7 @@ class AirsimROSWrapper std::vector cam_pub_vec_statistics; std::vector lidar_pub_vec_statistics; ros_bridge::Statistics imu_pub_statistics; + ros_bridge::Statistics gss_pub_statistics; // create std::vector which I can use to iterate over all these options // and apply common operations such as print, reset @@ -120,6 +122,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); @@ -188,6 +191,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 airsim_lidar_update_timers_; ros::Timer statistics_timer_; ros::Timer go_signal_timer_; @@ -201,6 +205,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 index b9c894f5..df217f3c 100644 --- a/ros/src/fsds_ros_bridge/launch/fsds_ros_bridge.launch +++ b/ros/src/fsds_ros_bridge/launch/fsds_ros_bridge.launch @@ -15,6 +15,7 @@ + diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp index aab5cfc2..691c6d65 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp @@ -41,10 +41,12 @@ void AirsimROSWrapper::initialize_statistics() getGpsDataStatistics = ros_bridge::Statistics("getGpsData"); getCarStateStatistics = ros_bridge::Statistics("getCarState"); getImuStatistics = ros_bridge::Statistics("getImu"); + getGSSStatistics = ros_bridge::Statistics("getGSS"); control_cmd_sub_statistics = ros_bridge::Statistics("control_cmd_sub"); global_gps_pub_statistics = ros_bridge::Statistics("global_gps_pub"); odom_pub_statistics = ros_bridge::Statistics("odom_pub"); imu_pub_statistics = ros_bridge::Statistics("imu_pub"); + gss_pub_statistics = ros_bridge::Statistics("gss_pub"); // Populate statistics obj vector // statistics_obj_ptr = {&setCarControlsStatistics, &getGpsDataStatistics, &getCarStateStatistics, &control_cmd_sub_statistics, &global_gps_pub_statistics, &odom_local_ned_pub_statistics}; @@ -98,12 +100,14 @@ void AirsimROSWrapper::initialize_ros() double update_odom_every_n_sec; double update_gps_every_n_sec; double update_imu_every_n_sec; + double update_gss_every_n_sec; double publish_static_tf_every_n_sec; nh_private_.getParam("mission_name", mission_name_); nh_private_.getParam("track_name", track_name_); nh_private_.getParam("update_odom_every_n_sec", update_odom_every_n_sec); nh_private_.getParam("update_gps_every_n_sec", update_gps_every_n_sec); nh_private_.getParam("update_imu_every_n_sec", update_imu_every_n_sec); + nh_private_.getParam("update_gss_every_n_sec", update_gss_every_n_sec); nh_private_.getParam("publish_static_tf_every_n_sec", publish_static_tf_every_n_sec); @@ -111,6 +115,7 @@ void AirsimROSWrapper::initialize_ros() odom_update_timer_ = nh_private_.createTimer(ros::Duration(update_odom_every_n_sec), &AirsimROSWrapper::odom_cb, this); gps_update_timer_ = nh_private_.createTimer(ros::Duration(update_gps_every_n_sec), &AirsimROSWrapper::gps_timer_cb, this); imu_update_timer_ = nh_private_.createTimer(ros::Duration(update_imu_every_n_sec), &AirsimROSWrapper::imu_timer_cb, this); + gss_update_timer_ = nh_private_.createTimer(ros::Duration(update_gss_every_n_sec), &AirsimROSWrapper::gss_timer_cb, this); statictf_timer_ = nh_private_.createTimer(ros::Duration(publish_static_tf_every_n_sec), &AirsimROSWrapper::statictf_cb, this); statistics_timer_ = nh_private_.createTimer(ros::Duration(1), &AirsimROSWrapper::statistics_timer_cb, this); @@ -147,6 +152,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() odom_pub = nh_.advertise("testing_only/odom", 10); global_gps_pub = nh_.advertise("gps", 10); imu_pub = nh_.advertise("imu", 10); + gss_pub = nh_.advertise("gss", 10); control_cmd_sub = nh_.subscribe("control_command", 1, boost::bind(&AirsimROSWrapper::car_control_cb, this, _1, vehicle_name)); // TODO: remove track publisher at competition track_pub = nh_.advertise("testing_only/track", 10, true); @@ -194,6 +200,11 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() std::cout << "Distance" << std::endl; break; } + case msr::airlib::SensorBase::SensorType::GSS: + { + std::cout << "Ground Speed Sensor" << std::endl; + break; + } case msr::airlib::SensorBase::SensorType::Lidar: { std::cout << "Lidar" << std::endl; @@ -214,7 +225,8 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() } default: { - throw std::invalid_argument("Unexpected sensor type"); + std::cout << sensor_setting->sensor_name << std::endl; + throw std::invalid_argument("Unexpected sensor type (D)"); } } } @@ -495,6 +507,44 @@ void AirsimROSWrapper::imu_timer_cb(const ros::TimerEvent& event) } } +void AirsimROSWrapper::gss_timer_cb(const ros::TimerEvent& event) +{ + try + { + struct msr::airlib::GSSSimple::Output gss_data; + { + ros_bridge::Timer timer(&getGSSStatistics); + std::unique_lock lck(car_control_mutex_); + gss_data = airsim_client_.getGroundSpeedSensorData(vehicle_name); + lck.unlock(); + } + + geometry_msgs::TwistStamped gss_msg; + gss_msg.header.frame_id = "fsds/" + vehicle_name; + gss_msg.header.stamp = make_ts(gss_data.time_stamp); + + // Convert to ENU frame (minus signs) + gss_msg.twist.linear.x = gss_data.twist.linear.x(); + gss_msg.twist.linear.y = - gss_data.twist.linear.y(); + gss_msg.twist.linear.z = - gss_data.twist.linear.z(); + gss_msg.twist.angular.x = gss_data.twist.angular.x(); + gss_msg.twist.angular.y = - gss_data.twist.angular.y(); + gss_msg.twist.angular.z = - gss_data.twist.angular.z(); + + { + ros_bridge::ROSMsgCounter counter(&gss_pub_statistics); + gss_pub.publish(gss_msg); + } + } + catch (rpc::rpc_error& e) + { + std::cout << "error" << std::endl; + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API while getting gss data:" << std::endl + << msg << std::endl; + } +} + void AirsimROSWrapper::statictf_cb(const ros::TimerEvent& event) { for (auto& static_tf_msg : static_tf_msg_vec_) @@ -655,10 +705,12 @@ void AirsimROSWrapper::PrintStatistics() getGpsDataStatistics.Print(); getCarStateStatistics.Print(); getImuStatistics.Print(); + getGSSStatistics.Print(); control_cmd_sub_statistics.Print(); global_gps_pub_statistics.Print(); odom_pub_statistics.Print(); imu_pub_statistics.Print(); + gss_pub_statistics.Print(); for (auto &getLidarDataStatistics : getLidarDataVecStatistics) { @@ -684,10 +736,12 @@ void AirsimROSWrapper::ResetStatistics() getGpsDataStatistics.Reset(); getCarStateStatistics.Reset(); getImuStatistics.Reset(); + getGSSStatistics.Reset(); control_cmd_sub_statistics.Reset(); global_gps_pub_statistics.Reset(); odom_pub_statistics.Reset(); imu_pub_statistics.Reset(); + gss_pub_statistics.Reset(); for (auto &getLidarDataStatistics : getLidarDataVecStatistics) { diff --git a/settings.json b/settings.json index 13611ece..be220e0a 100644 --- a/settings.json +++ b/settings.json @@ -54,6 +54,10 @@ "HorizontalFOVEnd": 90, "RotationsPerSecond": 5, "DrawDebugPoints": false + }, + "GSS" : { + "SensorType": 7, + "Enabled": true } }, "Cameras": { From ebef93a5b76856bd4c11da61077cce9d2aac4c31 Mon Sep 17 00:00:00 2001 From: sijmenhuizenga Date: Sun, 19 Jul 2020 14:39:00 +0200 Subject: [PATCH 2/5] remove angular velocity from ground speed sensor --- AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp | 9 +++------ AirSim/AirLib/include/sensors/gss/GSSSimple.hpp | 4 ++-- ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp | 9 +++------ 3 files changed, 8 insertions(+), 14 deletions(-) diff --git a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp index 1a31eddd..278aad62 100644 --- a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -761,9 +761,8 @@ class RpcLibAdapatorsBase { msr::airlib::TTimePoint time_stamp; Vector3r linear_velocity; - Vector3r angular_velocity; - MSGPACK_DEFINE_MAP(time_stamp, linear_velocity, angular_velocity); + MSGPACK_DEFINE_MAP(time_stamp, linear_velocity); GSSData() {} @@ -771,15 +770,13 @@ class RpcLibAdapatorsBase { GSSData(const msr::airlib::GSSSimple::Output& o) { time_stamp = o.time_stamp; - linear_velocity = o.twist.linear; - angular_velocity = o.twist.angular; + linear_velocity = o.linear_velocity; } msr::airlib::GSSSimple::Output to() const { msr::airlib::GSSSimple::Output s; - s.twist.linear = linear_velocity.to(); - s.twist.angular = angular_velocity.to(); + s.linear_velocity = linear_velocity.to(); s.time_stamp = time_stamp; return s; diff --git a/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp b/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp index 29fe5d15..c293793f 100644 --- a/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp +++ b/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp @@ -17,7 +17,7 @@ class GSSSimple : public SensorBase { public: struct Output { TTimePoint time_stamp; - Twist twist; + Vector3r linear_velocity; }; public: @@ -27,7 +27,7 @@ class GSSSimple : public SensorBase { const GroundTruth& ground_truth = getGroundTruth(); output.time_stamp = clock()->nowNanos(); - output.twist = ground_truth.kinematics->twist; + output.linear_velocity = ground_truth.kinematics->twist.linear; output_ = output; } diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp index 691c6d65..bf07f0bb 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp @@ -524,12 +524,9 @@ void AirsimROSWrapper::gss_timer_cb(const ros::TimerEvent& event) gss_msg.header.stamp = make_ts(gss_data.time_stamp); // Convert to ENU frame (minus signs) - gss_msg.twist.linear.x = gss_data.twist.linear.x(); - gss_msg.twist.linear.y = - gss_data.twist.linear.y(); - gss_msg.twist.linear.z = - gss_data.twist.linear.z(); - gss_msg.twist.angular.x = gss_data.twist.angular.x(); - gss_msg.twist.angular.y = - gss_data.twist.angular.y(); - gss_msg.twist.angular.z = - gss_data.twist.angular.z(); + gss_msg.twist.linear.x = gss_data.linear_velocity.x(); + gss_msg.twist.linear.y = - gss_data.linear_velocity.y(); + gss_msg.twist.linear.z = - gss_data.linear_velocity.z(); { ros_bridge::ROSMsgCounter counter(&gss_pub_statistics); From 4e6a392079ff66a95eb93196e82dfa761eda764a Mon Sep 17 00:00:00 2001 From: sijmenhuizenga Date: Sun, 19 Jul 2020 14:41:39 +0200 Subject: [PATCH 3/5] Add gss docs --- docs/ros-bridge.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/ros-bridge.md b/docs/ros-bridge.md index e2652a70..a9e113b3 100644 --- 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 | From a146dca75b428bc8a9c8f70c80350327c152295f Mon Sep 17 00:00:00 2001 From: sijmenhuizenga Date: Tue, 21 Jul 2020 11:41:56 +0200 Subject: [PATCH 4/5] Publish gss information relative to the vehicle frame --- AirSim/AirLib/include/sensors/gss/GSSSimple.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp b/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp index c293793f..8e978fc5 100644 --- a/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp +++ b/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp @@ -27,7 +27,7 @@ class GSSSimple : public SensorBase { const GroundTruth& ground_truth = getGroundTruth(); output.time_stamp = clock()->nowNanos(); - output.linear_velocity = ground_truth.kinematics->twist.linear; + 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; } From c268bb60cd3f10da6f780ba715f2559f9f9bbcc6 Mon Sep 17 00:00:00 2001 From: sijmenhuizenga Date: Tue, 21 Jul 2020 12:08:32 +0200 Subject: [PATCH 5/5] Added ground speed sensor docs --- docs/ground-speed-sensor.md | 31 +++++++++++++++++++++++++++++++ mkdocs.yml | 1 + 2 files changed, 32 insertions(+) create mode 100644 docs/ground-speed-sensor.md diff --git a/docs/ground-speed-sensor.md b/docs/ground-speed-sensor.md new file mode 100644 index 00000000..316e572e --- /dev/null +++ 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 +``` \ No newline at end of file diff --git a/mkdocs.yml b/mkdocs.yml index da8c3814..d2550c35 100644 --- 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"