From 0dceabb0a30937b09000838745927dd649b99300 Mon Sep 17 00:00:00 2001
From: Marcel Dudek <43888122+MarcelDudek@users.noreply.github.com>
Date: Wed, 11 May 2022 02:31:27 +0200
Subject: [PATCH] gazebo_plugins: GPS sensor plugin publishing velocity (#1371)

* GPS sensor publishing velocity

Added velocity publisher to GPS sensor plugin. Velocity is published as Vector3Stamped message in ENU coordinates.

Since Foxy supports Gazebo 11 by default, GPS sensor implements VelocityEast, VelocityNorth and VelocityUp methods
that allow for velocity simulation.

* GPS sensor velocity tests

Testing velocity messages provided by GPS plugin.
---
 gazebo_plugins/CMakeLists.txt                 |  1 +
 .../gazebo_plugins/gazebo_ros_gps_sensor.hpp  |  1 +
 gazebo_plugins/src/gazebo_ros_gps_sensor.cpp  | 19 ++++++++++--
 .../test/test_gazebo_ros_gps_sensor.cpp       | 30 +++++++++++++++++--
 .../test/worlds/gazebo_ros_gps_sensor.world   | 15 ++++++++++
 5 files changed, 60 insertions(+), 6 deletions(-)

diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt
index 9032e8842..7b1464e68 100644
--- a/gazebo_plugins/CMakeLists.txt
+++ b/gazebo_plugins/CMakeLists.txt
@@ -186,6 +186,7 @@ target_include_directories(gazebo_ros_gps_sensor PUBLIC include)
 ament_target_dependencies(gazebo_ros_gps_sensor
   "gazebo_ros"
   "sensor_msgs"
+  "geometry_msgs"
   "gazebo_dev"
 )
 ament_export_libraries(gazebo_ros_gps_sensor)
diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gps_sensor.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gps_sensor.hpp
index fdb88434a..b17d52646 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gps_sensor.hpp
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gps_sensor.hpp
@@ -40,6 +40,7 @@ class GazeboRosGpsSensorPrivate;
           <!-- publish to /gps/data -->
           <namespace>/gps</namespace>
           <remapping>~/out:=data</remapping>
+          <remapping>~/vel:=velocity</remapping>
         </ros>
       </plugin>
     </sensor>
diff --git a/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp b/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp
index 58c9b8c6f..b22cdcdaa 100644
--- a/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp
+++ b/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp
@@ -23,6 +23,7 @@
 #include <ignition/common/Profiler.hh>
 #endif
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 
 #include <iostream>
 #include <memory>
@@ -38,8 +39,12 @@ class GazeboRosGpsSensorPrivate
   gazebo_ros::Node::SharedPtr ros_node_;
   /// Publish for gps message
   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr pub_;
+  /// Publish for velocity message
+  rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr vel_pub_;
   /// GPS message modified each update
   sensor_msgs::msg::NavSatFix::SharedPtr msg_;
+  /// Velocity message modified each update
+  geometry_msgs::msg::Vector3Stamped::SharedPtr msg_vel_;
   /// GPS sensor this plugin is attached to
   gazebo::sensors::GpsSensorPtr sensor_;
   /// Event triggered when sensor updates
@@ -73,12 +78,15 @@ void GazeboRosGpsSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt
 
   impl_->pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::NavSatFix>(
     "~/out", qos.get_publisher_qos("~/out", rclcpp::SensorDataQoS().reliable()));
+  impl_->vel_pub_ = impl_->ros_node_->create_publisher<geometry_msgs::msg::Vector3Stamped>(
+    "~/vel", qos.get_publisher_qos("~/vel", rclcpp::SensorDataQoS().reliable()));
 
   // Create message to be reused
   auto msg = std::make_shared<sensor_msgs::msg::NavSatFix>();
+  auto msg_vel = std::make_shared<geometry_msgs::msg::Vector3Stamped>();
 
   // Get frame for message
-  msg->header.frame_id = gazebo_ros::SensorFrameID(*_sensor, *_sdf);
+  msg->header.frame_id = msg_vel->header.frame_id = gazebo_ros::SensorFrameID(*_sensor, *_sdf);
 
   // Fill covariances
   using SNT = gazebo::sensors::SensorNoiseType;
@@ -95,6 +103,7 @@ void GazeboRosGpsSensor::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPt
   msg->status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
 
   impl_->msg_ = msg;
+  impl_->msg_vel_ = msg_vel;
 
   impl_->sensor_update_event_ = impl_->sensor_->ConnectUpdated(
     std::bind(&GazeboRosGpsSensorPrivate::OnUpdate, impl_.get()));
@@ -106,12 +115,15 @@ void GazeboRosGpsSensorPrivate::OnUpdate()
   IGN_PROFILE("GazeboRosGpsSensorPrivate::OnUpdate");
   IGN_PROFILE_BEGIN("fill ROS message");
   #endif
-  // Fill message with latest sensor data
-  msg_->header.stamp = gazebo_ros::Convert<builtin_interfaces::msg::Time>(
+  // Fill messages with the latest sensor data
+  msg_->header.stamp = msg_vel_->header.stamp = gazebo_ros::Convert<builtin_interfaces::msg::Time>(
     sensor_->LastUpdateTime());
   msg_->latitude = sensor_->Latitude().Degree();
   msg_->longitude = sensor_->Longitude().Degree();
   msg_->altitude = sensor_->Altitude();
+  msg_vel_->vector.x = sensor_->VelocityEast();
+  msg_vel_->vector.y = sensor_->VelocityNorth();
+  msg_vel_->vector.z = sensor_->VelocityUp();
 
   #ifdef IGN_PROFILER_ENABLE
   IGN_PROFILE_END();
@@ -119,6 +131,7 @@ void GazeboRosGpsSensorPrivate::OnUpdate()
   #endif
   // Publish message
   pub_->publish(*msg_);
+  vel_pub_->publish(*msg_vel_);
   #ifdef IGN_PROFILER_ENABLE
   IGN_PROFILE_END();
   #endif
diff --git a/gazebo_plugins/test/test_gazebo_ros_gps_sensor.cpp b/gazebo_plugins/test/test_gazebo_ros_gps_sensor.cpp
index ac0f3ad65..5a736b03d 100644
--- a/gazebo_plugins/test/test_gazebo_ros_gps_sensor.cpp
+++ b/gazebo_plugins/test/test_gazebo_ros_gps_sensor.cpp
@@ -17,10 +17,12 @@
 #include <gazebo_ros/testing_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 
 #include <memory>
 
 #define tol 10e-4
+#define tol_vel 10e-3
 
 /// Tests the gazebo_ros_gps_sensor plugin
 class GazeboRosGpsSensorTest : public gazebo::ServerFixture
@@ -54,16 +56,25 @@ TEST_F(GazeboRosGpsSensorTest, GpsMessageCorrect)
     [&msg](sensor_msgs::msg::NavSatFix::SharedPtr _msg) {
       msg = _msg;
     });
+  geometry_msgs::msg::Vector3Stamped::SharedPtr msg_vel = nullptr;
+  auto sub_vel = node->create_subscription<geometry_msgs::msg::Vector3Stamped>(
+    "/gps/velocity", rclcpp::SensorDataQoS(),
+    [&msg_vel](geometry_msgs::msg::Vector3Stamped::SharedPtr _msg) {
+      msg_vel = _msg;
+    });
 
   world->Step(1);
   EXPECT_NEAR(0.0, box->WorldPose().Pos().X(), tol);
   EXPECT_NEAR(0.0, box->WorldPose().Pos().Y(), tol);
   EXPECT_NEAR(0.5, box->WorldPose().Pos().Z(), tol);
+  EXPECT_NEAR(0.0, box->WorldLinearVel().X(), tol_vel);
+  EXPECT_NEAR(0.0, box->WorldLinearVel().Y(), tol_vel);
+  EXPECT_NEAR(0.0, box->WorldLinearVel().Z(), tol_vel);
 
   // Step until a gps message will have been published
   int sleep{0};
   int max_sleep{1000};
-  while (sleep < max_sleep && nullptr == msg) {
+  while (sleep < max_sleep && (nullptr == msg || nullptr == msg_vel)) {
     world->Step(100);
     rclcpp::spin_some(node);
     gazebo::common::Time::MSleep(100);
@@ -72,22 +83,30 @@ TEST_F(GazeboRosGpsSensorTest, GpsMessageCorrect)
   EXPECT_LT(0u, sub->get_publisher_count());
   EXPECT_LT(sleep, max_sleep);
   ASSERT_NE(nullptr, msg);
+  ASSERT_NE(nullptr, msg_vel);
 
   // Get the initial gps output when the box is at rest
   auto pre_movement_msg = std::make_shared<sensor_msgs::msg::NavSatFix>(*msg);
+  auto pre_movement_msg_vel = std::make_shared<geometry_msgs::msg::Vector3Stamped>(*msg_vel);
   ASSERT_NE(nullptr, pre_movement_msg);
   EXPECT_NEAR(0.0, pre_movement_msg->latitude, tol);
   EXPECT_NEAR(0.0, pre_movement_msg->longitude, tol);
   EXPECT_NEAR(0.5, pre_movement_msg->altitude, tol);
+  EXPECT_NEAR(0.0, pre_movement_msg_vel->vector.x, tol_vel);
+  EXPECT_NEAR(0.0, pre_movement_msg_vel->vector.y, tol_vel);
+  EXPECT_NEAR(0.0, pre_movement_msg_vel->vector.z, tol_vel);
 
-  // Change the position of the link and step a few times to wait the ros message to be received
+  // Change the position and velocity of the link
+  // and step a few times to wait the ros message to be received
   msg = nullptr;
+  msg_vel = nullptr;
   ignition::math::Pose3d box_pose;
   box_pose.Pos() = {100.0, 200.0, 300.0};
   link->SetWorldPose(box_pose);
+  link->SetLinearVel({15.0, 10.0, 0.0});
 
   sleep = 0;
-  while (sleep < max_sleep && (nullptr == msg || msg->altitude < 150)) {
+  while (sleep < max_sleep && (nullptr == msg || msg->altitude < 150 || nullptr == msg_vel)) {
     world->Step(50);
     rclcpp::spin_some(node);
     gazebo::common::Time::MSleep(100);
@@ -96,10 +115,15 @@ TEST_F(GazeboRosGpsSensorTest, GpsMessageCorrect)
 
   // Check that GPS output reflects the position change
   auto post_movement_msg = std::make_shared<sensor_msgs::msg::NavSatFix>(*msg);
+  auto post_movement_msg_vel = std::make_shared<geometry_msgs::msg::Vector3Stamped>(*msg_vel);
   ASSERT_NE(nullptr, post_movement_msg);
+  ASSERT_NE(nullptr, post_movement_msg_vel);
   EXPECT_GT(post_movement_msg->latitude, 0.0);
   EXPECT_GT(post_movement_msg->longitude, 0.0);
   EXPECT_NEAR(300, post_movement_msg->altitude, 1);
+  EXPECT_NEAR(15.0, post_movement_msg_vel->vector.x, tol_vel);
+  EXPECT_NEAR(10.0, post_movement_msg_vel->vector.y, tol_vel);
+  EXPECT_NEAR(0.0, post_movement_msg_vel->vector.z, 0.1);
 }
 
 int main(int argc, char ** argv)
diff --git a/gazebo_plugins/test/worlds/gazebo_ros_gps_sensor.world b/gazebo_plugins/test/worlds/gazebo_ros_gps_sensor.world
index 8777de731..57aac17ce 100644
--- a/gazebo_plugins/test/worlds/gazebo_ros_gps_sensor.world
+++ b/gazebo_plugins/test/worlds/gazebo_ros_gps_sensor.world
@@ -48,11 +48,26 @@
                 </noise>
               </vertical>
             </position_sensing>
+            <velocity_sensing>
+              <horizontal>
+                <noise type="gaussian">
+                  <mean>0.0</mean>
+                  <stddev>2e-4</stddev>
+                </noise>
+              </horizontal>
+              <vertical>
+                <noise type="gaussian">
+                  <mean>0.0</mean>
+                  <stddev>2e-4</stddev>
+                </noise>
+              </vertical>
+            </velocity_sensing>
           </gps>
           <plugin name="my_gps_plugin" filename="libgazebo_ros_gps_sensor.so">
             <ros>
               <namespace>/gps</namespace>
               <remapping>~/out:=data</remapping>
+              <remapping>~/vel:=velocity</remapping>
             </ros>
           </plugin>
         </sensor>