Sync upstream (#3)

* fix: limits of steering speed control * gazebo_plugins: GPS sensor plugin publishing velocity (ros-simulation#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> * gazebo_ros: Add inline keyword to template definitions (ros-simulation#1367) * gazebo_ros: Add inline keyword to template definitions * Uncrustify Signed-off-by: Jacob Perron <jacob@openrobotics.org> Co-authored-by: Gerard Heshusius <gHeshusius@lely.com> Co-authored-by: Jacob Perron <jacob@openrobotics.org> * Use shell=False in when launching gazebo (ros-simulation#1384) Signed-off-by: Aditya <aditya050995@gmail.com> * Fix sim time test (ros-simulation#1390) Partial backport of ros-simulation#1380. Signed-off-by: Jacob Perron <jacob@openrobotics.org> Co-authored-by: Jacob Perron <jacob@openrobotics.org> Co-authored-by: Marcel Dudek <43888122+MarcelDudek@users.noreply.github.com> Co-authored-by: Gerard Heshusius <gh.heshusius@gmail.com> Co-authored-by: Gerard Heshusius <gHeshusius@lely.com> Co-authored-by: Aditya Pande <aditya050995@gmail.com>
brisa-robotics · Aug 5, 2022 · 8f87c06 · 8f87c06
1 parent a123baa
commit 8f87c06
Show file tree

Hide file tree

Showing 13 changed files with 107 additions and 24 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>

diff --git a/gazebo_ros/include/gazebo_ros/conversions/builtin_interfaces.hpp b/gazebo_ros/include/gazebo_ros/conversions/builtin_interfaces.hpp
@@ -28,6 +28,7 @@ namespace gazebo_ros
 /// \param[in] in Gazebo Time to convert.
 /// \return A ROS Time message with the same value as in
 template<>
+inline
 builtin_interfaces::msg::Time Convert(const gazebo::common::Time & in)
 {
   builtin_interfaces::msg::Time time;
@@ -40,6 +41,7 @@ builtin_interfaces::msg::Time Convert(const gazebo::common::Time & in)
 /// \param[in] in Gazebo Time message to convert.
 /// \return A ROS Time message with the same value as in
 template<>
+inline
 builtin_interfaces::msg::Time Convert(const gazebo::msgs::Time & in)
 {
   builtin_interfaces::msg::Time time;
@@ -53,6 +55,7 @@ builtin_interfaces::msg::Time Convert(const gazebo::msgs::Time & in)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const builtin_interfaces::msg::Time &)
 {
   T::ConversionNotImplemented;
@@ -62,6 +65,7 @@ T Convert(const builtin_interfaces::msg::Time &)
 /// \param[in] in ROS Time message to convert.
 /// \return A Gazebo Time with the same value as in
 template<>
+inline
 gazebo::common::Time Convert(const builtin_interfaces::msg::Time & in)
 {
   gazebo::common::Time time;
@@ -75,6 +79,7 @@ gazebo::common::Time Convert(const builtin_interfaces::msg::Time & in)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const builtin_interfaces::msg::Duration &)
 {
   T::ConversionNotImplemented;
@@ -84,6 +89,7 @@ T Convert(const builtin_interfaces::msg::Duration &)
 /// \param[in] in ROS Time message to convert.
 /// \return A Gazebo Time with the same value as in
 template<>
+inline
 gazebo::common::Time Convert(const builtin_interfaces::msg::Duration & in)
 {
   gazebo::common::Time time;

diff --git a/gazebo_ros/include/gazebo_ros/conversions/gazebo_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/gazebo_msgs.hpp
@@ -32,6 +32,7 @@ namespace gazebo_ros
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const gazebo::msgs::Contacts &)
 {
   T::ConversionNotImplemented;
@@ -41,6 +42,7 @@ T Convert(const gazebo::msgs::Contacts &)
 /// \param[in] in Input message;
 /// \return A ROS Contacts state message with the same data as the input message
 template<>
+inline
 gazebo_msgs::msg::ContactsState Convert(const gazebo::msgs::Contacts & in)
 {
   gazebo_msgs::msg::ContactsState contact_state_msg;

diff --git a/gazebo_ros/include/gazebo_ros/conversions/generic.hpp b/gazebo_ros/include/gazebo_ros/conversions/generic.hpp
@@ -35,6 +35,7 @@ static rclcpp::Logger conversions_logger = rclcpp::get_logger("gazebo_ros_conver
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const ignition::math::Vector3d &)
 {
   T::ConversionNotImplemented;
@@ -45,6 +46,7 @@ T Convert(const ignition::math::Vector3d &)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const ignition::math::Quaterniond &)
 {
   T::ConversionNotImplemented;
@@ -55,6 +57,7 @@ T Convert(const ignition::math::Quaterniond &)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const ignition::math::Pose3d &)
 {
   T::ConversionNotImplemented;
@@ -65,6 +68,7 @@ T Convert(const ignition::math::Pose3d &)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const gazebo::common::Time &)
 {
   T::ConversionNotImplemented;
@@ -74,6 +78,7 @@ T Convert(const gazebo::common::Time &)
 /// \param[in] in Gazebo Time to convert.
 /// \return A rclcpp::Time object with the same value as in
 template<>
+inline
 rclcpp::Time Convert(const gazebo::common::Time & in)
 {
   return rclcpp::Time(in.sec, in.nsec, rcl_clock_type_t::RCL_ROS_TIME);
@@ -84,6 +89,7 @@ rclcpp::Time Convert(const gazebo::common::Time & in)
 /// \return Conversion result
 /// \tparam T Output type
 template<class T>
+inline
 T Convert(const gazebo::msgs::Time &)
 {
   T::ConversionNotImplemented;