Humbly bringing the Joy to gazebo. (#353)

Enable using the humble gazebo bridge with Joy. Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
gazebosim · Jan 13, 2023 · 74dcf50 · 74dcf50
1 parent fa2a1ab
commit 74dcf50
Show file tree

Hide file tree

Showing 25 changed files with 140 additions and 16 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1 +1,2 @@
 **/__pycache__
+.vscode
diff --git a/ros_gz_bridge/README.md b/ros_gz_bridge/README.md
@@ -51,6 +51,7 @@ The following message types can be bridged for topics:
 | sensor_msgs/msg/Imu                  | ignition::msgs::IMU                    |
 | sensor_msgs/msg/Image                | ignition::msgs::Image                  |
 | sensor_msgs/msg/JointState           | ignition::msgs::Model                  |
+| sensor_msgs/msg/Joy                  | ignition::msgs::Joy                  |
 | sensor_msgs/msg/LaserScan            | ignition::msgs::LaserScan              |
 | sensor_msgs/msg/MagneticField        | ignition::msgs::Magnetometer           |
 | sensor_msgs/msg/NavSatFixed          | ignition::msgs::NavSat               |

diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/sensor_msgs.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/sensor_msgs.hpp
@@ -21,6 +21,7 @@
 #include <ignition/msgs/fluid_pressure.pb.h>
 #include <ignition/msgs/image.pb.h>
 #include <ignition/msgs/imu.pb.h>
+#include <ignition/msgs/joy.pb.h>
 #include <ignition/msgs/laserscan.pb.h>
 #include <ignition/msgs/magnetometer.pb.h>
 #include <ignition/msgs/model.pb.h>
@@ -34,6 +35,7 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <sensor_msgs/msg/joy.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
@@ -45,6 +47,18 @@ namespace ros_gz_bridge
 {
 
 // sensor_msgs
+template<>
+void
+convert_ros_to_gz(
+  const sensor_msgs::msg::Joy & ros_msg,
+  ignition::msgs::Joy & gz_msg);
+
+template<>
+void
+convert_gz_to_ros(
+  const ignition::msgs::Joy & gz_msg,
+  sensor_msgs::msg::Joy & ros_msg);
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py
@@ -72,6 +72,7 @@
         Mapping('Image', 'Image'),
         Mapping('Imu', 'IMU'),
         Mapping('JointState', 'Model'),
+        Mapping('Joy', 'Joy'),
         Mapping('LaserScan', 'LaserScan'),
         Mapping('MagneticField', 'Magnetometer'),
         Mapping('NavSatFix', 'NavSat'),

diff --git a/ros_gz_bridge/src/convert/sensor_msgs.cpp b/ros_gz_bridge/src/convert/sensor_msgs.cpp
@@ -323,6 +323,40 @@ convert_gz_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const sensor_msgs::msg::Joy & ros_msg,
+  ignition::msgs::Joy & gz_msg)
+{
+  convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_header()));
+
+  for (auto i = 0u; i < ros_msg.axes.size(); ++i) {
+    gz_msg.add_axes(ros_msg.axes[i]);
+  }
+
+  for (auto i = 0u; i < ros_msg.buttons.size(); ++i) {
+    gz_msg.add_buttons(ros_msg.buttons[i]);
+  }
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const ignition::msgs::Joy & gz_msg,
+  sensor_msgs::msg::Joy & ros_msg)
+{
+  convert_gz_to_ros(gz_msg.header(), ros_msg.header);
+
+  for (auto i = 0; i < gz_msg.axes_size(); ++i) {
+    ros_msg.axes.push_back(gz_msg.axes(i));
+  }
+
+  for (auto i = 0; i < gz_msg.buttons_size(); ++i) {
+    ros_msg.buttons.push_back(gz_msg.buttons(i));
+  }
+}
+
 template<>
 void
 convert_ros_to_gz(

diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp
@@ -816,6 +816,35 @@ void compareTestMsg(const std::shared_ptr<ignition::msgs::Model> & _msg)
   }
 }
 
+void createTestMsg(ignition::msgs::Joy & _msg)
+{
+  ignition::msgs::Header header_msg;
+
+  createTestMsg(header_msg);
+  _msg.mutable_header()->CopyFrom(header_msg);
+
+  for (int i = 0u; i < 3; ++i) {
+    _msg.add_axes(0.5);
+    _msg.add_buttons(0);
+  }
+}
+
+void compareTestMsg(const std::shared_ptr<ignition::msgs::Joy> & _msg)
+{
+  ignition::msgs::Joy expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(std::make_shared<ignition::msgs::Header>(_msg->header()));
+
+  for (int i = 0; i < expected_msg.axes_size(); ++i) {
+    EXPECT_FLOAT_EQ(expected_msg.axes(i), _msg->axes(i));
+  }
+
+  for (int i = 0; i < expected_msg.buttons_size(); ++i) {
+    EXPECT_EQ(expected_msg.buttons(i), _msg->buttons(i));
+  }
+}
+
 void createTestMsg(ignition::msgs::LaserScan & _msg)
 {
   ignition::msgs::Header header_msg;

diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp
@@ -38,6 +38,7 @@
 #include <ignition/msgs/int32.pb.h>
 #include <ignition/msgs/joint_trajectory.pb.h>
 #include <ignition/msgs/joint_wrench.pb.h>
+#include <ignition/msgs/joy.pb.h>
 #include <ignition/msgs/laserscan.pb.h>
 #include <ignition/msgs/light.pb.h>
 #include <ignition/msgs/magnetometer.pb.h>
@@ -257,6 +258,14 @@ void createTestMsg(ignition::msgs::JointWrench & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<ignition::msgs::JointWrench> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(ignition::msgs::Joy & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<ignition::msgs::Joy> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(ignition::msgs::Entity & _msg);

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp
@@ -961,6 +961,32 @@ void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::JointState> & _msg)
   }
 }
 
+void createTestMsg(sensor_msgs::msg::Joy & _msg)
+{
+  std_msgs::msg::Header header_msg;
+  createTestMsg(header_msg);
+
+  _msg.header = header_msg;
+  _msg.axes = {0.5, 0.5, 0.5};
+  _msg.buttons = {0, 0, 0};
+}
+
+void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::Joy> & _msg)
+{
+  sensor_msgs::msg::Joy expected_msg;
+  createTestMsg(expected_msg);
+
+  compareTestMsg(_msg->header);
+
+  for (auto i = 0u; i < _msg->axes.size(); ++i) {
+    EXPECT_FLOAT_EQ(expected_msg.buttons[i], _msg->buttons[i]);
+  }
+
+  for (auto i = 0u; i < _msg->buttons.size(); ++i) {
+    EXPECT_EQ(expected_msg.buttons[i], _msg->buttons[i]);
+  }
+}
+
 void createTestMsg(sensor_msgs::msg::LaserScan & _msg)
 {
   const unsigned int num_readings = 100u;

diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp
@@ -63,6 +63,7 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <sensor_msgs/msg/joy.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
@@ -459,6 +460,14 @@ void createTestMsg(sensor_msgs::msg::JointState & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::JointState> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(sensor_msgs::msg::Joy & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<sensor_msgs::msg::Joy> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(sensor_msgs::msg::LaserScan & _msg);

diff --git a/ros_ign_gazebo_demos/launch/air_pressure.launch.py b/ros_ign_gazebo_demos/launch/air_pressure.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/battery.launch.py b/ros_ign_gazebo_demos/launch/battery.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/camera.launch.py b/ros_ign_gazebo_demos/launch/camera.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/depth_camera.launch.py b/ros_ign_gazebo_demos/launch/depth_camera.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/diff_drive.launch.py b/ros_ign_gazebo_demos/launch/diff_drive.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/gpu_lidar.launch.py b/ros_ign_gazebo_demos/launch/gpu_lidar.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/gpu_lidar_bridge.launch.py b/ros_ign_gazebo_demos/launch/gpu_lidar_bridge.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/image_bridge.launch.py b/ros_ign_gazebo_demos/launch/image_bridge.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/imu.launch.py b/ros_ign_gazebo_demos/launch/imu.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/joint_states.launch.py b/ros_ign_gazebo_demos/launch/joint_states.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/magnetometer.launch.py b/ros_ign_gazebo_demos/launch/magnetometer.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/rgbd_camera.launch.py b/ros_ign_gazebo_demos/launch/rgbd_camera.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/rgbd_camera_bridge.launch.py b/ros_ign_gazebo_demos/launch/rgbd_camera_bridge.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/robot_description_publisher.launch.py b/ros_ign_gazebo_demos/launch/robot_description_publisher.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated!Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/tf_bridge.launch.py b/ros_ign_gazebo_demos/launch/tf_bridge.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
 

diff --git a/ros_ign_gazebo_demos/launch/triggered_camera.launch.py b/ros_ign_gazebo_demos/launch/triggered_camera.launch.py
@@ -22,7 +22,7 @@
 
 
 def generate_launch_description():
-    print("ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!")
+    print('ros_ign_gazebo_demos is deprecated! Please use ros_gz_sim_demos instead!')
 
     pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')