Merge pull request #21 from hoffmann-stefan/feature/bridge-log

Bridge /rosout

CMakeLists.txt

@@ -105,7 +105,8 @@ set(actionlib_msgs_DEPRECATED_QUIET TRUE)
 foreach(package_name ${ros2_interface_packages})
   find_package(${package_name} QUIET REQUIRED)
   message(STATUS "Found ${package_name}: ${${package_name}_VERSION} (${${package_name}_DIR})")
-  if(NOT "${package_name}" STREQUAL "builtin_interfaces")
+  if(NOT "${package_name}" STREQUAL "builtin_interfaces" AND
+      NOT "${package_name}" STREQUAL "rcl_interfaces")
     list(APPEND generated_files "${generated_path}/${package_name}_factories.cpp")
     list(APPEND generated_files "${generated_path}/${package_name}_factories.hpp")
     foreach(interface_file ${${package_name}_IDL_FILES})

include/ros1_bridge/builtin_interfaces_factories.hpp

@@ -16,12 +16,14 @@
 #define ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_
 
 // include ROS 1 messages
+#include <rosgraph_msgs/Log.h>
 #include <std_msgs/Duration.h>
 #include <std_msgs/Time.h>
 
 // include ROS 2 messages
 #include <builtin_interfaces/msg/duration.hpp>
 #include <builtin_interfaces/msg/time.hpp>
+#include <rcl_interfaces/msg/log.hpp>
 
 #include <memory>
 #include <string>
@@ -74,6 +76,24 @@ Factory<
   const builtin_interfaces::msg::Time & ros2_msg,
   std_msgs::Time & ros1_msg);
 
+template<>
+void
+Factory<
+  rosgraph_msgs::Log,
+  rcl_interfaces::msg::Log
+>::convert_1_to_2(
+  const rosgraph_msgs::Log & ros1_msg,
+  rcl_interfaces::msg::Log & ros2_msg);
+
+template<>
+void
+Factory<
+  rosgraph_msgs::Log,
+  rcl_interfaces::msg::Log
+>::convert_2_to_1(
+  const rcl_interfaces::msg::Log & ros2_msg,
+  rosgraph_msgs::Log & ros1_msg);
+
 }  // namespace ros1_bridge
 
 #endif  // ROS1_BRIDGE__BUILTIN_INTERFACES_FACTORIES_HPP_

include/ros1_bridge/convert_builtin_interfaces.hpp

@@ -18,12 +18,14 @@
 #include "ros1_bridge/convert_decl.hpp"
 
 // include ROS 1 builtin messages
+#include "rosgraph_msgs/Log.h"
 #include "ros/duration.h"
 #include "ros/time.h"
 
 // include ROS 2 builtin messages
 #include "builtin_interfaces/msg/duration.hpp"
 #include "builtin_interfaces/msg/time.hpp"
+#include "rcl_interfaces/msg/log.hpp"
 
 namespace ros1_bridge
 {
@@ -52,6 +54,18 @@ convert_2_to_1(
   const builtin_interfaces::msg::Time & ros2_msg,
   ros::Time & ros1_type);
 
+template<>
+void
+convert_1_to_2(
+  const rosgraph_msgs::Log & ros1_msg,
+  rcl_interfaces::msg::Log & ros2_msg);
+
+template<>
+void
+convert_2_to_1(
+  const rcl_interfaces::msg::Log & ros2_msg,
+  rosgraph_msgs::Log & ros1_msg);
+
 }  // namespace ros1_bridge
 
 #endif  // ROS1_BRIDGE__CONVERT_BUILTIN_INTERFACES_HPP_

ros1_bridge/__init__.py

@@ -98,6 +98,9 @@ def generate_cpp(output_path, template_dir):
         data['ros2_package_names_msg'] + data['ros2_package_names_srv'])
     # skip builtin_interfaces since there is a custom implementation
     unique_package_names -= {'builtin_interfaces'}
+    # skip rcl_interfaces since there is a custom implementation
+    # Only for Log which is the only type that is useful to map.
+    unique_package_names -= {'rcl_interfaces'}
     data['ros2_package_names'] = list(unique_package_names)
 
     template_file = os.path.join(template_dir, 'get_factory.cpp.em')
@@ -531,9 +534,7 @@ def __init__(self, data, expected_package_name):
             self.ros1_package_name = data['ros1_package_name']
             self.ros2_package_name = data['ros2_package_name']
             self.foreign_mapping = bool(data.get('enable_foreign_mappings'))
-            self.package_mapping = (
-                len(data) == (2 + int('enable_foreign_mappings' in data))
-            )
+            self.package_mapping = self.ros1_package_name != self.ros2_package_name
         else:
             raise MappingException(
                 'Ignoring a rule without a ros1_package_name and/or ros2_package_name')
@@ -1095,6 +1096,10 @@ def determine_field_mapping(ros1_msg, ros2_msg, mapping_rules, msg_idx):
         # check that no fields exist in ROS 2 but not in ROS 1
         # since then it might be the case that those have been renamed and should be mapped
         for ros2_member in ros2_spec.structure.members:
+            if any(ros2_member == key[0] for key in mapping.fields_2_to_1.keys()):
+                # If they are explicitly mapped this should be fine...
+                continue
+
             for ros1_field in ros1_spec.parsed_fields():
                 if ros1_field.name.lower() == ros2_member.name:
                     break

src/builtin_interfaces_factories.cpp

@@ -52,6 +52,18 @@ get_factory_builtin_interfaces(
       >
     >("std_msgs/Time", ros2_type_name);
   }
+  if (
+    (ros1_type_name == "rosgraph_msgs/Log" || ros1_type_name == "") &&
+    ros2_type_name == "rcl_interfaces/msg/Log")
+  {
+    return std::make_shared<
+      Factory<
+        rosgraph_msgs::Log,
+        rcl_interfaces::msg::Log
+      >
+    >("rosgraph_msgs/Log", ros2_type_name);
+  }
+
   return std::shared_ptr<FactoryInterface>();
 }
 
@@ -104,4 +116,28 @@ Factory<
   ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg.data);
 }
 
+template<>
+void
+Factory<
+  rosgraph_msgs::Log,
+  rcl_interfaces::msg::Log
+>::convert_1_to_2(
+  const rosgraph_msgs::Log & ros1_msg,
+  rcl_interfaces::msg::Log & ros2_msg)
+{
+  ros1_bridge::convert_1_to_2(ros1_msg, ros2_msg);
+}
+
+template<>
+void
+Factory<
+  rosgraph_msgs::Log,
+  rcl_interfaces::msg::Log
+>::convert_2_to_1(
+  const rcl_interfaces::msg::Log & ros2_msg,
+  rosgraph_msgs::Log & ros1_msg)
+{
+  ros1_bridge::convert_2_to_1(ros2_msg, ros1_msg);
+}
+
 }  // namespace ros1_bridge

src/convert_builtin_interfaces.cpp

@@ -58,4 +58,91 @@ convert_2_to_1(
   ros1_type.nsec = ros2_msg.nanosec;
 }
 
+
+template<>
+void
+convert_1_to_2(
+  const rosgraph_msgs::Log & ros1_msg,
+  rcl_interfaces::msg::Log & ros2_msg)
+{
+  ros1_bridge::convert_1_to_2(ros1_msg.header.stamp, ros2_msg.stamp);
+
+  // Explicitly map the values of the log level as they differ between
+  // ROS1 and ROS2.
+  switch (ros1_msg.level) {
+    case rosgraph_msgs::Log::DEBUG:
+      ros2_msg.level = rcl_interfaces::msg::Log::DEBUG;
+      break;
+
+    case rosgraph_msgs::Log::INFO:
+      ros2_msg.level = rcl_interfaces::msg::Log::INFO;
+      break;
+
+    case rosgraph_msgs::Log::WARN:
+      ros2_msg.level = rcl_interfaces::msg::Log::WARN;
+      break;
+
+    case rosgraph_msgs::Log::ERROR:
+      ros2_msg.level = rcl_interfaces::msg::Log::ERROR;
+      break;
+
+    case rosgraph_msgs::Log::FATAL:
+      ros2_msg.level = rcl_interfaces::msg::Log::FATAL;
+      break;
+
+    default:
+      ros2_msg.level = ros1_msg.level;
+      break;
+  }
+
+  ros2_msg.name = ros1_msg.name;
+  ros2_msg.msg = ros1_msg.msg;
+  ros2_msg.file = ros1_msg.file;
+  ros2_msg.function = ros1_msg.function;
+  ros2_msg.line = ros1_msg.line;
+}
+
+template<>
+void
+convert_2_to_1(
+  const rcl_interfaces::msg::Log & ros2_msg,
+  rosgraph_msgs::Log & ros1_msg)
+{
+  ros1_bridge::convert_2_to_1(ros2_msg.stamp, ros1_msg.header.stamp);
+
+  // Explicitly map the values of the log level as they differ between
+  // ROS1 and ROS2.
+  switch (ros2_msg.level) {
+    case rcl_interfaces::msg::Log::DEBUG:
+      ros1_msg.level = rosgraph_msgs::Log::DEBUG;
+      break;
+
+    case rcl_interfaces::msg::Log::INFO:
+      ros1_msg.level = rosgraph_msgs::Log::INFO;
+      break;
+
+    case rcl_interfaces::msg::Log::WARN:
+      ros1_msg.level = rosgraph_msgs::Log::WARN;
+      break;
+
+    case rcl_interfaces::msg::Log::ERROR:
+      ros1_msg.level = rosgraph_msgs::Log::ERROR;
+      break;
+
+    case rcl_interfaces::msg::Log::FATAL:
+      ros1_msg.level = rosgraph_msgs::Log::FATAL;
+      break;
+
+    default:
+      ros1_msg.level = ros1_msg.level;
+      break;
+  }
+
+  ros1_msg.name = ros2_msg.name;
+  ros1_msg.msg = ros2_msg.msg;
+  ros1_msg.file = ros2_msg.file;
+  ros1_msg.function = ros2_msg.function;
+  ros1_msg.line = ros2_msg.line;
+}
+
 }  // namespace ros1_bridge