Skip to content

Commit

Permalink
gazebo_plugins: GPS sensor plugin publishing velocity (ros-simulation…
Browse files Browse the repository at this point in the history 
…#1371) (ros-simulation#1386)

* 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.

Co-authored-by: Marcel Dudek <43888122+MarcelDudek@users.noreply.github.com>
  • Loading branch information
@jacobperron @MarcelDudek @afonsofonbraga
2 people authored and afonsofonbraga committed Aug 4, 2022
1 parent 5cf978b commit d35e0d9
Show file tree
Hide file tree
Showing 5 changed files with 60 additions and 6 deletions.
1 change: 1 addition & 0 deletions gazebo_plugins/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions gazebo_plugins/include/gazebo_plugins/gazebo_ros_gps_sensor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@ class GazeboRosGpsSensorPrivate;
<!-- publish to /gps/data -->
<namespace>/gps</namespace>
<remapping>~/out:=data</remapping>
<remapping>~/vel:=velocity</remapping>
</ros>
</plugin>
</sensor>
Expand Down
19 changes: 16 additions & 3 deletions gazebo_plugins/src/gazebo_ros_gps_sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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
Expand Down Expand Up @@ -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;
Expand All @@ -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()));
Expand All @@ -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
Expand Down
30 changes: 27 additions & 3 deletions gazebo_plugins/test/test_gazebo_ros_gps_sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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)
Expand Down
15 changes: 15 additions & 0 deletions gazebo_plugins/test/worlds/gazebo_ros_gps_sensor.world
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down

0 comments on commit d35e0d9

Please sign in to comment.