From 4d936b1446eceacb3763525074e7d3407f85aba9 Mon Sep 17 00:00:00 2001 From: Mateusz Lichota Date: Thu, 21 Jul 2022 09:14:43 -0300 Subject: [PATCH 1/3] Make gss report linear and angular velocities --- AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp | 5 ++++- AirSim/AirLib/include/sensors/gss/GSSSimple.hpp | 7 +++++++ docker/README.md | 2 +- ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp | 4 ++++ 4 files changed, 16 insertions(+), 2 deletions(-) diff --git a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp index 8179c4dd..96bcf93a 100644 --- a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -656,8 +656,9 @@ class RpcLibAdapatorsBase { msr::airlib::TTimePoint time_stamp; Vector3r linear_velocity; + Vector3r angular_velocity; - MSGPACK_DEFINE_MAP(time_stamp, linear_velocity); + MSGPACK_DEFINE_MAP(time_stamp, linear_velocity, angular_velocity); GSSData() {} @@ -666,12 +667,14 @@ class RpcLibAdapatorsBase { { time_stamp = o.time_stamp; linear_velocity = o.linear_velocity; + angular_velocity = o.angular_velocity; } msr::airlib::GSSSimple::Output to() const { msr::airlib::GSSSimple::Output s; s.linear_velocity = linear_velocity.to(); + s.angular_velocity = angular_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 55da7cd9..1a6f2f62 100644 --- a/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp +++ b/AirSim/AirLib/include/sensors/gss/GSSSimple.hpp @@ -18,6 +18,7 @@ class GSSSimple : public SensorBase { struct Output { TTimePoint time_stamp; Vector3r linear_velocity; + Vector3r angular_velocity; }; public: @@ -28,8 +29,14 @@ class GSSSimple : public SensorBase { const GroundTruth& ground_truth = getGroundTruth(); output.time_stamp = clock()->nowNanos(); + output.angular_velocity = ground_truth.kinematics->twist.angular; output.linear_velocity = ground_truth.kinematics->twist.linear; + // velocity is in world frame so transform to body frame + output.linear_velocity = VectorMath::transformToBodyFrame( + output.linear_velocity, ground_truth.kinematics->pose.orientation, true + ); + output_ = output; } const Output& getOutput() const diff --git a/docker/README.md b/docker/README.md index 274249e1..22e65962 100644 --- a/docker/README.md +++ b/docker/README.md @@ -1 +1 @@ -See ../docs/docker_ubuntu.md \ No newline at end of file +See ../docs/docker_ubuntu.md diff --git a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp index e403c50c..c8597ece 100644 --- a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp +++ b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp @@ -542,6 +542,10 @@ void AirsimROSWrapper::gss_timer_cb() gss_msg.header.frame_id = "fsds/" + vehicle_name; gss_msg.header.stamp = make_ts(gss_data.time_stamp); + gss_msg.twist.angular.x = gss_data.angular_velocity.x(); + gss_msg.twist.angular.y = gss_data.angular_velocity.y(); + gss_msg.twist.angular.z = gss_data.angular_velocity.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(); From 38e3d6d4319b51bb2f1b742195ec45e007cdb7b4 Mon Sep 17 00:00:00 2001 From: Mateusz Lichota Date: Thu, 21 Jul 2022 10:00:10 -0300 Subject: [PATCH 2/3] Make gss use TwistWithCovarianceStamped Closes #198 --- docs/ros-bridge.md | 4 ++-- .../config/multiplot/multiplot.xml | 18 +++++++------- .../include/airsim_ros_wrapper.h | 4 ++-- .../src/airsim_ros_wrapper.cpp | 24 ++++++++++++------- 4 files changed, 29 insertions(+), 21 deletions(-) diff --git a/docs/ros-bridge.md b/docs/ros-bridge.md index 6dc15a22..e67b68ce 100644 --- a/docs/ros-bridge.md +++ b/docs/ros-bridge.md @@ -22,7 +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/gss` | Ground speed sensor provide linear velocity of the vehicle (`fsds/FSCar`). Velocity is m/s. | [geometry_msgs/TwistWithCovarianceStamped](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. | [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). | [fs_msgs/Track](https://github.com/FS-Driverless/fs_msgs/blob/master/msg/Track.msg) | Latched | |`/fsds/testing_only/extra_info`|This topic contains extra information about the simulator. At the moment these are the doo_counter, that keeps track of the amount of cones that have been hit, and the times of the laps|[fs_msgs/ExtraInfo](https://github.com/FS-Driverless/fs_msgs/blob/master/msg/ExtraInfo.msg)|1| @@ -122,4 +122,4 @@ To open Rviz with [this](https://github.com/FS-Driverless/Formula-Student-Driver To open Multiplot with [this](https://github.com/FS-Driverless/Formula-Student-Driverless-Simulator/blob/master/ros/src/fsds_ros_bridge/config/multiplot/multiplot.xml) configuration file, run `roslaunch fsds_ros_bridge fsds_ros_bridge.launch plot:=true`. ## Monitoring -Performance monitoring of the ROS Bridge is described [here](statistics.md) \ No newline at end of file +Performance monitoring of the ROS Bridge is described [here](statistics.md) diff --git a/ros2/src/fsds_ros2_bridge/config/multiplot/multiplot.xml b/ros2/src/fsds_ros2_bridge/config/multiplot/multiplot.xml index 3ec4e92f..8788655e 100644 --- a/ros2/src/fsds_ros2_bridge/config/multiplot/multiplot.xml +++ b/ros2/src/fsds_ros2_bridge/config/multiplot/multiplot.xml @@ -783,10 +783,10 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped - twist/linear/x + twist/twist/linear/x 0 1000 @@ -796,7 +796,7 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped @@ -834,10 +834,10 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped - twist/linear/y + twist/twist/linear/y 0 1000 @@ -847,7 +847,7 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped @@ -885,10 +885,10 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped - twist/linear/z + twist/twist/linear/z 0 1000 @@ -898,7 +898,7 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped diff --git a/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h b/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h index 6f213e36..24f766f4 100644 --- a/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h +++ b/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h @@ -23,7 +23,7 @@ STRICT_MODE_OFF //todo what does this do? #include #include #include -#include +#include #include #include #include @@ -217,7 +217,7 @@ class AirsimROSWrapper std::shared_ptr> odom_pub; std::shared_ptr> global_gps_pub; std::shared_ptr> imu_pub; - std::shared_ptr> gss_pub; + std::shared_ptr> gss_pub; std::shared_ptr> track_pub; std::shared_ptr> go_signal_pub_; std::shared_ptr> extra_info_pub; diff --git a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp index c8597ece..15f49a57 100644 --- a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp +++ b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp @@ -186,7 +186,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() clock_pub = nh_->create_publisher("/clock", 10); global_gps_pub = nh_->create_publisher("gps", 10); imu_pub = nh_->create_publisher("imu", 10); - gss_pub = nh_->create_publisher("gss", 10); + gss_pub = nh_->create_publisher("gss", 10); bool UDP_control; nh_->get_parameter("UDP_control", UDP_control); @@ -538,17 +538,25 @@ void AirsimROSWrapper::gss_timer_cb() lck.unlock(); } - geometry_msgs::msg::TwistStamped gss_msg; + geometry_msgs::msg::TwistWithCovarianceStamped gss_msg; gss_msg.header.frame_id = "fsds/" + vehicle_name; gss_msg.header.stamp = make_ts(gss_data.time_stamp); - gss_msg.twist.angular.x = gss_data.angular_velocity.x(); - gss_msg.twist.angular.y = gss_data.angular_velocity.y(); - gss_msg.twist.angular.z = gss_data.angular_velocity.z(); + gss_msg.twist.twist.angular.x = gss_data.angular_velocity.x(); + gss_msg.twist.twist.angular.y = gss_data.angular_velocity.y(); + gss_msg.twist.twist.angular.z = gss_data.angular_velocity.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(); + gss_msg.twist.twist.linear.x = gss_data.linear_velocity.x(); + gss_msg.twist.twist.linear.y = gss_data.linear_velocity.y(); + gss_msg.twist.twist.linear.z = gss_data.linear_velocity.z(); + + // The 0.1 covariances for everything were just guessed, don't assume theese are correct + gss_msg.twist.covariance[0] = 0.1; + gss_msg.twist.covariance[7] = 0.1; + gss_msg.twist.covariance[14] = 0.1; + gss_msg.twist.covariance[21] = 0.1; + gss_msg.twist.covariance[28] = 0.1; + gss_msg.twist.covariance[35] = 0.1; { ros_bridge::ROSMsgCounter counter(&gss_pub_statistics); From a3945186c6b0d7346c7859d547edeff25db6b189 Mon Sep 17 00:00:00 2001 From: Wouter Heerwegh Date: Sat, 6 Aug 2022 13:19:41 +0200 Subject: [PATCH 3/3] Add changes to ROS1 --- .../config/multiplot/multiplot.xml | 18 +++++++-------- .../include/airsim_ros_wrapper.h | 2 +- .../src/airsim_ros_wrapper.cpp | 22 ++++++++++++++----- 3 files changed, 27 insertions(+), 15 deletions(-) diff --git a/ros/src/fsds_ros_bridge/config/multiplot/multiplot.xml b/ros/src/fsds_ros_bridge/config/multiplot/multiplot.xml index 3ec4e92f..8788655e 100644 --- a/ros/src/fsds_ros_bridge/config/multiplot/multiplot.xml +++ b/ros/src/fsds_ros_bridge/config/multiplot/multiplot.xml @@ -783,10 +783,10 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped - twist/linear/x + twist/twist/linear/x 0 1000 @@ -796,7 +796,7 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped @@ -834,10 +834,10 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped - twist/linear/y + twist/twist/linear/y 0 1000 @@ -847,7 +847,7 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped @@ -885,10 +885,10 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped - twist/linear/z + twist/twist/linear/z 0 1000 @@ -898,7 +898,7 @@ 0 /fsds/gss - geometry_msgs/TwistStamped + geometry_msgs/TwistWithCovarianceStamped 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 866ccee4..29c73d58 100644 --- a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h +++ b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h @@ -23,7 +23,7 @@ STRICT_MODE_OFF //todo what does this do? #include #include #include -#include +#include #include #include #include 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 a357f175..55986dc3 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp @@ -178,7 +178,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() vehicle_name = curr_vehicle_name; global_gps_pub = nh_.advertise("gps", 10); imu_pub = nh_.advertise("imu", 10); - gss_pub = nh_.advertise("gss", 10); + gss_pub = nh_.advertise("gss", 10); bool UDP_control; nh_private_.getParam("UDP_control", UDP_control); @@ -532,13 +532,25 @@ void AirsimROSWrapper::gss_timer_cb(const ros::TimerEvent& event) lck.unlock(); } - geometry_msgs::TwistStamped gss_msg; + geometry_msgs::TwistWithCovarianceStamped gss_msg; gss_msg.header.frame_id = "fsds/" + vehicle_name; gss_msg.header.stamp = make_ts(gss_data.time_stamp); - 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(); + gss_msg.twist.twist.angular.x = gss_data.angular_velocity.x(); + gss_msg.twist.twist.angular.y = gss_data.angular_velocity.y(); + gss_msg.twist.twist.angular.z = gss_data.angular_velocity.z(); + + gss_msg.twist.twist.linear.x = gss_data.linear_velocity.x(); + gss_msg.twist.twist.linear.y = gss_data.linear_velocity.y(); + gss_msg.twist.twist.linear.z = gss_data.linear_velocity.z(); + + // The 0.1 covariances for everything were just guessed, don't assume theese are correct + gss_msg.twist.covariance[0] = 0.1; + gss_msg.twist.covariance[7] = 0.1; + gss_msg.twist.covariance[14] = 0.1; + gss_msg.twist.covariance[21] = 0.1; + gss_msg.twist.covariance[28] = 0.1; + gss_msg.twist.covariance[35] = 0.1; { ros_bridge::ROSMsgCounter counter(&gss_pub_statistics);