Merge pull request #304 from mateusz-lichota/fixing_gss

Fixing GSS
FS-Driverless · Aug 15, 2022 · aeae1c7 · aeae1c7
2 parents f37660b + 90ded54
commit aeae1c7
Show file tree

Hide file tree

Showing 10 changed files with 69 additions and 35 deletions.
diff --git a/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirSim/AirLib/include/api/RpcLibAdapatorsBase.hpp
@@ -729,8 +729,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()
  {}
@@ -739,12 +740,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
@@ -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
@@ -1 +1 @@
-See ../docs/docker_ubuntu.md
+See ../docs/docker_ubuntu.md
diff --git 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)
+Performance monitoring of the ROS Bridge is described [here](statistics.md)
diff --git a/ros/src/fsds_ros_bridge/config/multiplot/multiplot.xml b/ros/src/fsds_ros_bridge/config/multiplot/multiplot.xml
@@ -783,10 +783,10 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </x_axis>
  <y_axis>
- <field>twist/linear/x</field>
+ <field>twist/twist/linear/x</field>
  <field_type>0</field_type>
  <scale>
  <absolute_maximum>1000</absolute_maximum>
@@ -796,7 +796,7 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </y_axis>
  </axes>
  <color>
@@ -834,10 +834,10 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </x_axis>
  <y_axis>
- <field>twist/linear/y</field>
+ <field>twist/twist/linear/y</field>
  <field_type>0</field_type>
  <scale>
  <absolute_maximum>1000</absolute_maximum>
@@ -847,7 +847,7 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </y_axis>
  </axes>
  <color>
@@ -885,10 +885,10 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </x_axis>
  <y_axis>
- <field>twist/linear/z</field>
+ <field>twist/twist/linear/z</field>
  <field_type>0</field_type>
  <scale>
  <absolute_maximum>1000</absolute_maximum>
@@ -898,7 +898,7 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </y_axis>
  </axes>
  <color>

diff --git 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 <cv_bridge/cv_bridge.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/TwistWithCovarianceStamped.h>
 #include <iostream>
 #include <math.h>
 #include <math_common.h>

diff --git 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<sensor_msgs::NavSatFix>("gps", 10);
  imu_pub = nh_.advertise<sensor_msgs::Imu>("imu", 10);
- gss_pub = nh_.advertise<geometry_msgs::TwistStamped>("gss", 10);
+ gss_pub = nh_.advertise<geometry_msgs::TwistWithCovarianceStamped>("gss", 10);
 
  bool UDP_control;
  nh_private_.getParam("UDP_control", UDP_control);
@@ -521,13 +521,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);

diff --git a/ros2/src/fsds_ros2_bridge/config/multiplot/multiplot.xml b/ros2/src/fsds_ros2_bridge/config/multiplot/multiplot.xml
@@ -783,10 +783,10 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </x_axis>
  <y_axis>
- <field>twist/linear/x</field>
+ <field>twist/twist/linear/x</field>
  <field_type>0</field_type>
  <scale>
  <absolute_maximum>1000</absolute_maximum>
@@ -796,7 +796,7 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </y_axis>
  </axes>
  <color>
@@ -834,10 +834,10 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </x_axis>
  <y_axis>
- <field>twist/linear/y</field>
+ <field>twist/twist/linear/y</field>
  <field_type>0</field_type>
  <scale>
  <absolute_maximum>1000</absolute_maximum>
@@ -847,7 +847,7 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </y_axis>
  </axes>
  <color>
@@ -885,10 +885,10 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </x_axis>
  <y_axis>
- <field>twist/linear/z</field>
+ <field>twist/twist/linear/z</field>
  <field_type>0</field_type>
  <scale>
  <absolute_maximum>1000</absolute_maximum>
@@ -898,7 +898,7 @@
  <type>0</type>
  </scale>
  <topic>/fsds/gss</topic>
- <type>geometry_msgs/TwistStamped</type>
+ <type>geometry_msgs/TwistWithCovarianceStamped</type>
  </y_axis>
  </axes>
  <color>

diff --git a/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h b/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h
@@ -24,7 +24,7 @@ STRICT_MODE_OFF //todo what does this do?
 #include <cv_bridge/cv_bridge.h>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 #include <iostream>
@@ -222,7 +222,7 @@ class AirsimROSWrapper
  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odom_pub;
  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::NavSatFix>> global_gps_pub;
  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Imu>> imu_pub;
- std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>> gss_pub;
+ std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>> gss_pub;
  std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::WheelStates>> wheel_states_pub;
  std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::Track>> track_pub;
  std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::GoSignal>> go_signal_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
@@ -192,7 +192,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
  clock_pub = nh_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
  global_gps_pub = nh_->create_publisher<sensor_msgs::msg::NavSatFix>("gps", 10);
  imu_pub = nh_->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
- gss_pub = nh_->create_publisher<geometry_msgs::msg::TwistStamped>("gss", 10);
+ gss_pub = nh_->create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>("gss", 10);
  wheel_states_pub = nh_->create_publisher<fs_msgs::msg::WheelStates>("wheel_states", 10);
 
  bool UDP_control;
@@ -543,13 +543,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.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);