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.
ros-simulation · May 11, 2022 · 9bb4fdb · 9bb4fdb
1 parent a212ce3
commit 9bb4fdb
Show file tree

Hide file tree

Showing 5 changed files with 60 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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,19 +115,23 @@ 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();
   IGN_PROFILE_BEGIN("publish");
   #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
@@ -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
@@ -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>