[ros2] Add JointTrajectory message conversion (#121)

Conversion between - ignition::msgs::JointTrajectory - trajectory_msgs::msg::JointTrajectory Signed-off-by: Andrej Orsula <orsula.andrej@gmail.com>
gazebosim · Jan 15, 2021 · a4bc25f · a4bc25f
1 parent c059286
commit a4bc25f
Show file tree

Hide file tree

Showing 14 changed files with 431 additions and 2 deletions.
diff --git a/ros_ign_bridge/CMakeLists.txt b/ros_ign_bridge/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(rosgraph_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
 
 # Dome
 if("$ENV{IGNITION_VERSION}" STREQUAL "dome")
@@ -71,6 +72,7 @@ ament_target_dependencies(${bridge_lib}
   "sensor_msgs"
   "std_msgs"
   "tf2_msgs"
+  "trajectory_msgs"
 )
 
 install(TARGETS ${bridge_lib}
@@ -103,6 +105,7 @@ foreach(bridge ${bridge_executables})
     "sensor_msgs"
     "std_msgs"
     "tf2_msgs"
+    "trajectory_msgs"
   )
   install(TARGETS ${bridge}
     DESTINATION lib/${PROJECT_NAME}
@@ -126,6 +129,7 @@ if(BUILD_TESTING)
     sensor_msgs
     std_msgs
     tf2_msgs
+    trajectory_msgs
   )
   target_link_libraries(test_ros_publisher
     ${GTEST_LIBRARIES}
@@ -143,6 +147,7 @@ if(BUILD_TESTING)
     sensor_msgs
     std_msgs
     tf2_msgs
+    trajectory_msgs
   )
 
   target_link_libraries(test_ros_subscriber

diff --git a/ros_ign_bridge/README.md b/ros_ign_bridge/README.md
@@ -36,6 +36,7 @@ service calls. Its support is limited to only the following message types:
 | sensor_msgs/msg/MagneticField      | ignition::msgs::Magnetometer     |
 | sensor_msgs/msg/PointCloud2        | ignition::msgs::PointCloudPacked |
 | tf2_msgs/msg/TFMessage             | ignition::msgs::Pose_V           |
+| trajectory_msgs/msg/JointTrajectory | ignition::msgs::JointTrajectory |
 
 Run `ros2 run ros_ign_bridge parameter_bridge -h` for instructions.
 

diff --git a/ros_ign_bridge/include/ros_ign_bridge/convert.hpp b/ros_ign_bridge/include/ros_ign_bridge/convert.hpp
@@ -43,6 +43,7 @@
 #include <std_msgs/msg/int32.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
 
 // Ignition messages
 #include <ignition/msgs.hh>
@@ -396,6 +397,32 @@ convert_ign_to_ros(
   const ignition::msgs::BatteryState & ign_msg,
   sensor_msgs::msg::BatteryState & ros_msg);
 
+// trajectory_msgs
+template<>
+void
+convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectoryPoint & ros_msg,
+  ignition::msgs::JointTrajectoryPoint & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::JointTrajectoryPoint & ign_msg,
+  trajectory_msgs::msg::JointTrajectoryPoint & ros_msg);
+
+template<>
+void
+convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectory & ros_msg,
+  ignition::msgs::JointTrajectory & ign_msg);
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::JointTrajectory & ign_msg,
+  trajectory_msgs::msg::JointTrajectory & ros_msg);
+
+
 }  // namespace ros_ign_bridge
 
 #endif  // ROS_IGN_BRIDGE__CONVERT_HPP_
diff --git a/ros_ign_bridge/package.xml b/ros_ign_bridge/package.xml
@@ -20,6 +20,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2_msgs</depend>
+  <depend>trajectory_msgs</depend>
 
   <!-- Dome -->
   <depend condition="$IGNITION_VERSION == dome">ignition-msgs6</depend>

diff --git a/ros_ign_bridge/src/convert.cpp b/ros_ign_bridge/src/convert.cpp
@@ -1055,4 +1055,89 @@ convert_ign_to_ros(
   ros_msg.present = true;
 }
 
+template<>
+void
+convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectoryPoint & ros_msg,
+  ignition::msgs::JointTrajectoryPoint & ign_msg)
+{
+  for (const auto & ros_position : ros_msg.positions) {
+    ign_msg.add_positions(ros_position);
+  }
+  for (const auto & ros_velocity : ros_msg.velocities) {
+    ign_msg.add_velocities(ros_velocity);
+  }
+  for (const auto & ros_acceleration : ros_msg.accelerations) {
+    ign_msg.add_accelerations(ros_acceleration);
+  }
+  for (const auto & ros_effort : ros_msg.effort) {
+    ign_msg.add_effort(ros_effort);
+  }
+
+  ignition::msgs::Duration * ign_duration = ign_msg.mutable_time_from_start();
+  ign_duration->set_sec(ros_msg.time_from_start.sec);
+  ign_duration->set_nsec(ros_msg.time_from_start.nanosec);
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::JointTrajectoryPoint & ign_msg,
+  trajectory_msgs::msg::JointTrajectoryPoint & ros_msg)
+{
+  for (auto i = 0; i < ign_msg.positions_size(); ++i) {
+    ros_msg.positions.push_back(ign_msg.positions(i));
+  }
+  for (auto i = 0; i < ign_msg.velocities_size(); ++i) {
+    ros_msg.velocities.push_back(ign_msg.velocities(i));
+  }
+  for (auto i = 0; i < ign_msg.accelerations_size(); ++i) {
+    ros_msg.accelerations.push_back(ign_msg.accelerations(i));
+  }
+  for (auto i = 0; i < ign_msg.effort_size(); ++i) {
+    ros_msg.effort.push_back(ign_msg.effort(i));
+  }
+
+  ros_msg.time_from_start = rclcpp::Duration(
+    ign_msg.time_from_start().sec(),
+    ign_msg.time_from_start().nsec());
+}
+
+template<>
+void
+convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectory & ros_msg,
+  ignition::msgs::JointTrajectory & ign_msg)
+{
+  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
+
+  for (const auto & ros_joint_name : ros_msg.joint_names) {
+    ign_msg.add_joint_names(ros_joint_name);
+  }
+
+  for (const auto & ros_point : ros_msg.points) {
+    ignition::msgs::JointTrajectoryPoint * ign_point = ign_msg.add_points();
+    convert_ros_to_ign(ros_point, (*ign_point));
+  }
+}
+
+template<>
+void
+convert_ign_to_ros(
+  const ignition::msgs::JointTrajectory & ign_msg,
+  trajectory_msgs::msg::JointTrajectory & ros_msg)
+{
+  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
+
+  for (auto i = 0; i < ign_msg.joint_names_size(); ++i) {
+    ros_msg.joint_names.push_back(ign_msg.joint_names(i));
+  }
+
+  for (auto i = 0; i < ign_msg.points_size(); ++i) {
+    trajectory_msgs::msg::JointTrajectoryPoint ros_point;
+    convert_ign_to_ros(ign_msg.points(i), ros_point);
+    ros_msg.points.push_back(ros_point);
+  }
+}
+
 }  // namespace ros_ign_bridge
diff --git a/ros_ign_bridge/src/factories.cpp b/ros_ign_bridge/src/factories.cpp
@@ -311,6 +311,16 @@ get_factory_impl(
       >
     >("sensor_msgs/msg/BatteryState", ign_type_name);
   }
+  if ((ros_type_name == "trajectory_msgs/msg/JointTrajectory" || ros_type_name.empty()) &&
+    ign_type_name == "ignition.msgs.JointTrajectory")
+  {
+    return std::make_shared<
+      Factory<
+        trajectory_msgs::msg::JointTrajectory,
+        ignition::msgs::JointTrajectory
+      >
+    >("trajectory_msgs/msg/JointTrajectory", ign_type_name);
+  }
   return std::shared_ptr<FactoryInterface>();
 }
 
@@ -1009,4 +1019,53 @@ Factory<
   ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
 }
 
+// trajectory_msgs
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectoryPoint,
+  ignition::msgs::JointTrajectoryPoint
+>::convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectoryPoint & ros_msg,
+  ignition::msgs::JointTrajectoryPoint & ign_msg)
+{
+  ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectoryPoint,
+  ignition::msgs::JointTrajectoryPoint
+>::convert_ign_to_ros(
+  const ignition::msgs::JointTrajectoryPoint & ign_msg,
+  trajectory_msgs::msg::JointTrajectoryPoint & ros_msg)
+{
+  ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
+}
+
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectory,
+  ignition::msgs::JointTrajectory
+>::convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectory & ros_msg,
+  ignition::msgs::JointTrajectory & ign_msg)
+{
+  ros_ign_bridge::convert_ros_to_ign(ros_msg, ign_msg);
+}
+
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectory,
+  ignition::msgs::JointTrajectory
+>::convert_ign_to_ros(
+  const ignition::msgs::JointTrajectory & ign_msg,
+  trajectory_msgs::msg::JointTrajectory & ros_msg)
+{
+  ros_ign_bridge::convert_ign_to_ros(ign_msg, ros_msg);
+}
+
 }  // namespace ros_ign_bridge
diff --git a/ros_ign_bridge/src/factories.hpp b/ros_ign_bridge/src/factories.hpp
@@ -41,6 +41,7 @@
 #include <std_msgs/msg/int32.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
 
 // Ignition messages
 #include <ignition/msgs.hh>
@@ -571,6 +572,44 @@ Factory<
   const ignition::msgs::BatteryState & ign_msg,
   sensor_msgs::msg::BatteryState & ros_msg);
 
+// trajectory_msgs
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectoryPoint,
+  ignition::msgs::JointTrajectoryPoint
+>::convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectoryPoint & ros_msg,
+  ignition::msgs::JointTrajectoryPoint & ign_msg);
+
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectoryPoint,
+  ignition::msgs::JointTrajectoryPoint
+>::convert_ign_to_ros(
+  const ignition::msgs::JointTrajectoryPoint & ign_msg,
+  trajectory_msgs::msg::JointTrajectoryPoint & ros_msg);
+
+
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectory,
+  ignition::msgs::JointTrajectory
+>::convert_ros_to_ign(
+  const trajectory_msgs::msg::JointTrajectory & ros_msg,
+  ignition::msgs::JointTrajectory & ign_msg);
+
+template<>
+void
+Factory<
+  trajectory_msgs::msg::JointTrajectory,
+  ignition::msgs::JointTrajectory
+>::convert_ign_to_ros(
+  const ignition::msgs::JointTrajectory & ign_msg,
+  trajectory_msgs::msg::JointTrajectory & ros_msg);
+
 }  // namespace ros_ign_bridge
 
 #endif  // FACTORIES_HPP_
diff --git a/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ign_subscriber.launch.py
@@ -65,7 +65,8 @@ def generate_test_description():
           '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry',
           '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
           '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model',
-          '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState'
+          '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState',
+          '/joint_trajectory@trajectory_msgs/msg/JointTrajectory@ignition.msgs.JointTrajectory'
         ],
         output='screen'
     )

diff --git a/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py b/ros_ign_bridge/test/launch/test_ros_subscriber.launch.py
@@ -65,7 +65,8 @@ def generate_test_description():
           '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry',
           '/pointcloud2@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
           '/joint_states@sensor_msgs/msg/JointState@ignition.msgs.Model',
-          '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState'
+          '/battery_state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState',
+          '/joint_trajectory@trajectory_msgs/msg/JointTrajectory@ignition.msgs.JointTrajectory'
         ],
         output='screen'
     )

diff --git a/ros_ign_bridge/test/publishers/ign_publisher.cpp b/ros_ign_bridge/test/publishers/ign_publisher.cpp
@@ -187,6 +187,11 @@ int main(int /*argc*/, char **/*argv*/)
   ignition::msgs::BatteryState battery_state_msg;
   ros_ign_bridge::testing::createTestMsg(battery_state_msg);
 
+  // ignition::msgs::JointTrajectory.
+  auto joint_trajectory_pub = node.Advertise<ignition::msgs::JointTrajectory>("joint_trajectory");
+  ignition::msgs::JointTrajectory joint_trajectory_msg;
+  ros_ign_bridge::testing::createTestMsg(joint_trajectory_msg);
+
   // Publish messages at 1Hz.
   while (!g_terminatePub) {
     bool_pub.Publish(bool_msg);
@@ -216,6 +221,7 @@ int main(int /*argc*/, char **/*argv*/)
     twist_pub.Publish(twist_msg);
     pointcloudpacked_pub.Publish(pointcloudpacked_msg);
     battery_state_pub.Publish(battery_state_msg);
+    joint_trajectory_pub.Publish(joint_trajectory_msg);
 
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }

diff --git a/ros_ign_bridge/test/publishers/ros_publisher.cpp b/ros_ign_bridge/test/publishers/ros_publisher.cpp
@@ -39,6 +39,7 @@
 #include <std_msgs/msg/header.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
 #include "../test_utils.hpp"
 
 //////////////////////////////////////////////////
@@ -203,6 +204,12 @@ int main(int argc, char ** argv)
   sensor_msgs::msg::BatteryState battery_state_msg;
   ros_ign_bridge::testing::createTestMsg(battery_state_msg);
 
+  // trajectory_msgs::msg::JointTrajectory.
+  auto joint_trajectory_pub =
+    node->create_publisher<trajectory_msgs::msg::JointTrajectory>("joint_trajectory", 1000);
+  trajectory_msgs::msg::JointTrajectory joint_trajectory_msg;
+  ros_ign_bridge::testing::createTestMsg(joint_trajectory_msg);
+
   while (rclcpp::ok()) {
     // Publish all messages.
     bool_pub->publish(bool_msg);
@@ -232,6 +239,7 @@ int main(int argc, char ** argv)
     joint_states_pub->publish(joint_states_msg);
     pointcloud2_pub->publish(pointcloud2_msg);
     battery_state_pub->publish(battery_state_msg);
+    joint_trajectory_pub->publish(joint_trajectory_msg);
 
     rclcpp::spin_some(node);
     loop_rate.sleep();

diff --git a/ros_ign_bridge/test/subscribers/ign_subscriber.cpp b/ros_ign_bridge/test/subscribers/ign_subscriber.cpp
@@ -379,6 +379,18 @@ TEST(IgnSubscriberTest, BatteryState)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+/////////////////////////////////////////////////
+TEST(IgnSubscriberTest, JointTrajectory)
+{
+  MyTestClass<ignition::msgs::JointTrajectory> client("joint_trajectory");
+
+  using namespace std::chrono_literals;
+  ros_ign_bridge::testing::waitUntilBoolVar(
+    client.callbackExecuted, 100ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 int main(int argc, char ** argv)
 {