ros-simulation · jacobperron · May 11, 2022 · May 11, 2022
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>