Added extra_info topic to ros bridge

docs/getting-started-with-ros.md

@@ -99,6 +99,7 @@ Now, all that is left to do is subscribe to the following topics to receive sens
 - `/fsds/lidar/LIDAR_NAME`
 - `/fsds/testing_only/odom`
 - `/fsds/testing_only/track`
+- `/fsds/testing_only/extra_info`
 
 and publish to the following topic `/fsds/control_command` to publish the vehicle control setpoints.
 

docs/ros-bridge.md

@@ -25,6 +25,7 @@ The fsds_ros_bridge.launch launches the following nodes:
 | `/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. | [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 number of laps|[fs_msgs/ExtraInfo](https://github.com/FS-Driverless/fs_msgs/blob/master/msg/ExtraInfo.msg)|1|
 | `/fsds/camera/CAMERA_NAME` | Camera images. See [./camera.md](camera docs). | [sensor_msgs/Image](https://docs.ros.org/api/sensor_msgs/html/msg/Image.html) | ~18 |
 | `/fsds/lidar/LIDARNAME` | Publishes the lidar points for each lidar sensor. All points are in the `fsds/LIDARNAME` frame. Transformations between the `fsds/LIDARNAME` and `fsds/FSCar` frame are being published regularly. More info on the lidar sensor can be found [here](lidar.md) | [sensor_msgs/PointCloud](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud.html) | ``RotationsPerSecond` param in `settings.json` |
 | `/fsds/signal/go` | GO signal that is sent every second by the ROS bridge.The car is only allowed to drive once this message has been received. If no GO signal is received for more than 4 seconds, the AS can assume that `fsds_ros_bridge` has been shut down. This message also includes the mission type and track. More info about signal topics can be found in the [competition-signals guide](competition-signals.md) | [fs_msgs/GoSignal](https://github.com/FS-Driverless/fs_msgs/blob/master/msg/GoSignal.msg) | 1 |

ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h

@@ -18,6 +18,7 @@ STRICT_MODE_OFF //todo what does this do?
 #include <fs_msgs/GoSignal.h>
 #include <fs_msgs/FinishedSignal.h>
 #include <fs_msgs/Track.h>
+#include <fs_msgs/ExtraInfo.h>
 #include <chrono>
 #include <cv_bridge/cv_bridge.h>
 #include <geometry_msgs/PoseStamped.h>
@@ -128,6 +129,7 @@ class AirsimROSWrapper
  void lidar_timer_cb(const ros::TimerEvent& event, const std::string& camera_name, const int lidar_index);
  void statistics_timer_cb(const ros::TimerEvent& event);
  void go_signal_timer_cb(const ros::TimerEvent& event);
+ void extra_info_cb(const ros::TimerEvent & event);
 
  /// ROS subscriber callbacks
  void finished_signal_cb(fs_msgs::FinishedSignalConstPtr msg);
@@ -203,6 +205,7 @@ class AirsimROSWrapper
  ros::Timer statistics_timer_;
  ros::Timer go_signal_timer_;
  ros::Timer statictf_timer_;
+ ros::Timer extra_info_timer_;
 
  std::vector<ros::Publisher> lidar_pub_vec_;
 
@@ -215,6 +218,7 @@ class AirsimROSWrapper
  ros::Publisher gss_pub;
  ros::Publisher track_pub;
  ros::Publisher go_signal_pub_;
+ ros::Publisher extra_info_pub;
 
  /// ROS subscribers
  ros::Subscriber control_cmd_sub;

ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp

@@ -132,6 +132,7 @@ void AirsimROSWrapper::initialize_ros()
 
  if(!competition_mode_) {
  odom_update_timer_ = nh_private_.createTimer(ros::Duration(update_odom_every_n_sec), &AirsimROSWrapper::odom_cb, this);
+ extra_info_timer_ = nh_private_.createTimer(ros::Duration(1), &AirsimROSWrapper::extra_info_cb, this);
  }
 
  gps_update_timer_ = nh_private_.createTimer(ros::Duration(update_gps_every_n_sec), &AirsimROSWrapper::gps_timer_cb, this);
@@ -191,6 +192,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
  if(!competition_mode_) {
  odom_pub = nh_.advertise<nav_msgs::Odometry>("testing_only/odom", 10);
  track_pub = nh_.advertise<fs_msgs::Track>("testing_only/track", 10, true);
+ extra_info_pub = nh_.advertise<fs_msgs::ExtraInfo>("testing_only/extra_info", 10, true);
  }
 
  // iterate over camera map std::map<std::string, CameraSetting> cameras;
@@ -818,6 +820,26 @@ void AirsimROSWrapper::finished_signal_cb(fs_msgs::FinishedSignalConstPtr msg)
  curl_global_cleanup();
 }
 
+void AirsimROSWrapper::extra_info_cb(const ros::TimerEvent & event){
+ CarApiBase::RefereeState state;
+ try{
+ std::unique_lock<std::recursive_mutex> lck(car_control_mutex_);
+ state = airsim_client_.getRefereeState();
+ lck.unlock();
+ } catch (rpc::rpc_error& e)
+ {
+ std::string msg = e.get_error().as<std::string>();
+ std::cout << "Exception raised by the API, something went wrong while retrieving referee state for sending extra info." << std::endl
+ << msg << std::endl;
+ }
+
+ fs_msgs::ExtraInfo extra_info_msg;
+ extra_info_msg.doo_counter = state.doo_counter;
+ extra_info_msg.laps = state.laps.size();
+
+ extra_info_pub.publish(extra_info_msg);
+}
+
 bool AirsimROSWrapper::equalsMessage(const nav_msgs::Odometry& a, const nav_msgs::Odometry& b) {
  return a.pose.pose.position.x == b.pose.pose.position.x &&
  a.pose.pose.position.y == b.pose.pose.position.y &&