From 9bb4fdb946a077b6bd40ec1ce1c6a287499d0826 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; /gps ~/out:=data + ~/vel:=velocity diff --git a/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp b/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp index 40c031229..b55d28341 100644 --- a/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_gps_sensor.cpp @@ -23,6 +23,7 @@ #include #endif #include +#include #include #include @@ -38,8 +39,12 @@ class GazeboRosGpsSensorPrivate gazebo_ros::Node::SharedPtr ros_node_; /// Publish for gps message rclcpp::Publisher::SharedPtr pub_; + /// Publish for velocity message + rclcpp::Publisher::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( "~/out", qos.get_publisher_qos("~/out", rclcpp::SensorDataQoS().reliable())); + impl_->vel_pub_ = impl_->ros_node_->create_publisher( + "~/vel", qos.get_publisher_qos("~/vel", rclcpp::SensorDataQoS().reliable())); // Create message to be reused auto msg = std::make_shared(); + auto msg_vel = std::make_shared(); // 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( + // Fill messages with the latest sensor data + msg_->header.stamp = msg_vel_->header.stamp = gazebo_ros::Convert( 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 #include #include +#include #include #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( + "/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(*msg); + auto pre_movement_msg_vel = std::make_shared(*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(*msg); + auto post_movement_msg_vel = std::make_shared(*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 @@ + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + /gps ~/out:=data + ~/vel:=velocity