diff --git a/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch
index a4246a61..4807a182 100644
--- a/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch
@@ -65,6 +65,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -97,6 +118,17 @@
args="$(arg sensor_prefix)/left_realsense/image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -129,6 +171,17 @@
args="$(arg sensor_prefix)/rear_realsense/image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -161,6 +224,17 @@
args="$(arg sensor_prefix)/right_realsense/image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch
index ba429f04..b4c31020 100644
--- a/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -77,6 +77,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
+
diff --git a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch
index fe30a5eb..6cf09a90 100644
--- a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch
@@ -77,8 +77,25 @@
+
+
+
+
+
+
-
+
+
+
+
+
+
diff --git a/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch
index 3b820aeb..31876fbf 100755
--- a/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -70,6 +70,16 @@
args="$(arg sensor_prefix)/camera_front/depth_image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -101,6 +122,16 @@
args="$(arg sensor_prefix)/camera_up/depth_image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -132,6 +174,16 @@
args="$(arg sensor_prefix)/camera_down/depth_image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -179,6 +242,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch
index 9e82ad3f..c3e42e5d 100755
--- a/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/ssci_x4_sensor_config_1/launch/vehicle_topics.launch
@@ -75,6 +75,16 @@
args="$(arg sensor_prefix)/camera_front/depth_image">
+
+
+
+
+
+
@@ -108,6 +118,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -126,6 +157,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
diff --git a/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch b/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch
index a59e43f0..f04d381d 100755
--- a/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch
+++ b/submitted_models/ssci_x4_sensor_config_2/launch/vehicle_topics.launch
@@ -75,6 +75,16 @@
args="$(arg sensor_prefix)/camera_front/depth_image">
+
+
+
+
+
+
@@ -108,6 +118,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -126,6 +157,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
diff --git a/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch b/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch
index 9d90f2e2..f52d4f7f 100644
--- a/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch
+++ b/submitted_models/x1_sensor_config_6/launch/vehicle_topics.launch
@@ -29,6 +29,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -98,6 +108,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -116,6 +147,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
diff --git a/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch b/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch
index e8fda481..f53784de 100644
--- a/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch
+++ b/submitted_models/x4_sensor_config_6/launch/vehicle_topics.launch
@@ -65,6 +65,16 @@
args="$(arg sensor_prefix)/camera_front/depth_image">
+
+
+
+
+
+
@@ -98,6 +108,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -116,6 +147,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
diff --git a/subt_ros/CMakeLists.txt b/subt_ros/CMakeLists.txt
index 000ca774..cd9c98ba 100644
--- a/subt_ros/CMakeLists.txt
+++ b/subt_ros/CMakeLists.txt
@@ -40,6 +40,11 @@ target_link_libraries(pose_tf_broadcaster
${catkin_LIBRARIES}
)
+add_executable(optical_frame_publisher src/OpticalFramePublisher.cc)
+target_link_libraries(optical_frame_publisher
+ ${catkin_LIBRARIES}
+)
+
add_executable(bridge_logger src/BridgeLogger.cc)
add_dependencies(bridge_logger ${catkin_EXPORTED_TARGETS})
target_link_libraries(bridge_logger ${catkin_LIBRARIES})
@@ -71,6 +76,7 @@ target_link_libraries(set_pose_relay
add_dependencies(set_pose_relay ${catkin_EXPORTED_TARGETS})
install(TARGETS pose_tf_broadcaster subt_ros_relay set_pose_relay bridge_logger
+ optical_frame_publisher
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch
diff --git a/subt_ros/launch/vehicle_topics.launch b/subt_ros/launch/vehicle_topics.launch
index 8e76f2f8..85ba9953 100644
--- a/subt_ros/launch/vehicle_topics.launch
+++ b/subt_ros/launch/vehicle_topics.launch
@@ -68,6 +68,16 @@
args="$(arg sensor_prefix)/camera_front/depth_image">
+
+
+
+
+
+
@@ -101,6 +111,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -119,6 +150,17 @@
args="$(arg sensor_prefix)/camera_front/image">
+
+
+
+
+
+
+
+
diff --git a/subt_ros/src/OpticalFramePublisher.cc b/subt_ros/src/OpticalFramePublisher.cc
new file mode 100644
index 00000000..f9a2ba59
--- /dev/null
+++ b/subt_ros/src/OpticalFramePublisher.cc
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include
+
+// include ROS 1
+#ifdef __clang__
+# pragma clang diagnostic push
+# pragma clang diagnostic ignored "-Wunused-parameter"
+#endif
+#include
+#ifdef __clang__
+# pragma clang diagnostic pop
+#endif
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+/// \brief A class that converts data from robot frame to optical frame
+/// It subscribes to existing topic, modifies the frame_id of the message to
+/// a new optical frame, then republishes the updated data to a new topic.
+class OpticalFramePublisher
+{
+ // Documentation inherited
+ public: void Init(bool _cameraInfo = true);
+
+ /// \brief Subscriber callback which updates the frame id of the message
+ /// and republishes the message.
+ /// \param[in] _msg Message whose frame id is to be updated
+ private: void UpdateImageFrame(const sensor_msgs::Image::Ptr &_msg);
+
+ /// \brief Subscriber callback which updates the frame id of the message
+ /// and republishes the message.
+ /// \param[in] _msg Message whose frame id is to be updated
+ private: void UpdateCameraInfoFrame(const sensor_msgs::CameraInfo::Ptr &_msg);
+
+ /// \brief Publish static tf data between the original frame of the msg
+ /// and the new optical frame
+ /// \param[in] _frame Original message frame id
+ /// \param[in] _childFrame Optical frame id
+ private: void PublishTF(const std::string &_frame,
+ const std::string &_childFrame);
+
+ /// \brief Callback when subscriber connects to the image topic
+ private: void ImageConnect();
+
+ /// \brief Callback when subscriber connects to the camera info topic
+ private: void CameraInfoConnect();
+
+ /// \brief ROS node handle
+ private: ros::NodeHandle node;
+
+ /// \brief ROS subscriber that subscribes to original image topic
+ private: std::unique_ptr sub;
+
+ /// \brief ROS subscriber that subscribes to original camera info topic
+ private: std::unique_ptr ciSub;
+
+ /// \brief ROS publisher that publishes image msg with the new optical frame
+ private: ros::Publisher pub;
+
+ /// \brief ROS publisher that publishes camera info msg with the new optical
+ /// frame
+ private: ros::Publisher ciPub;
+
+ /// \brief Optical frame id
+ private: std::string newFrameId;
+};
+
+//////////////////////////////////////////////////
+void OpticalFramePublisher::Init(bool _cameraInfo)
+{
+ this->pub = this->node.advertise("output/image", 10,
+ std::bind(&OpticalFramePublisher::ImageConnect, this));
+
+ if (_cameraInfo)
+ {
+ this->ciPub = this->node.advertise(
+ "output/camera_info", 10,
+ std::bind(&OpticalFramePublisher::CameraInfoConnect, this));
+ }
+
+ ROS_INFO("Optical Frame Publisher Ready");
+}
+
+//////////////////////////////////////////////////
+void OpticalFramePublisher::UpdateImageFrame(
+ const sensor_msgs::Image::Ptr &_msg)
+{
+ if (this->pub.getNumSubscribers() == 0u && this->sub)
+ {
+ this->sub.reset();
+ return;
+ }
+
+ if (this->newFrameId.empty())
+ {
+ this->newFrameId = _msg->header.frame_id + "_optical";
+ this->PublishTF(_msg->header.frame_id, this->newFrameId);
+ }
+
+ _msg->header.frame_id = this->newFrameId;
+ this->pub.publish(_msg);
+}
+
+//////////////////////////////////////////////////
+void OpticalFramePublisher::UpdateCameraInfoFrame(
+ const sensor_msgs::CameraInfo::Ptr &_msg)
+{
+ if (this->ciPub.getNumSubscribers() == 0u && this->ciSub)
+ {
+ this->ciSub.reset();
+ return;
+ }
+
+ if (this->newFrameId.empty())
+ {
+ this->newFrameId = _msg->header.frame_id + "_optical";
+ this->PublishTF(_msg->header.frame_id, this->newFrameId);
+ }
+
+ _msg->header.frame_id = this->newFrameId;
+ this->ciPub.publish(_msg);
+}
+
+//////////////////////////////////////////////////
+void OpticalFramePublisher::PublishTF(const std::string &_frame,
+ const std::string &_childFrame)
+{
+ geometry_msgs::TransformStamped tfStamped;
+ tfStamped.header.frame_id = _frame;
+ tfStamped.child_frame_id = _childFrame;
+ tfStamped.transform.translation.x = 0.0;
+ tfStamped.transform.translation.y = 0.0;
+ tfStamped.transform.translation.z = 0.0;
+ tf2::Quaternion q;
+ // converts x forward to z forward
+ q.setRPY(-M_PI/2.0, 0, -M_PI/2.0);
+ tfStamped.transform.rotation.x = q.x();
+ tfStamped.transform.rotation.y = q.y();
+ tfStamped.transform.rotation.z = q.z();
+ tfStamped.transform.rotation.w = q.w();
+
+ static tf2_ros::StaticTransformBroadcaster brStatic;
+ brStatic.sendTransform(tfStamped);
+}
+
+//////////////////////////////////////////////////
+void OpticalFramePublisher::ImageConnect()
+{
+ if (this->sub)
+ return;
+ this->sub = std::make_unique(
+ this->node.subscribe("input/image", 10,
+ &OpticalFramePublisher::UpdateImageFrame, this));
+}
+
+//////////////////////////////////////////////////
+void OpticalFramePublisher::CameraInfoConnect()
+{
+ if (this->ciSub)
+ return;
+ this->ciSub = std::make_unique(
+ this->node.subscribe("input/camera_info", 10,
+ &OpticalFramePublisher::UpdateCameraInfoFrame, this));
+}
+
+//////////////////////////////////////////////////
+int main(int _argc, char **_argv)
+{
+ std::string type;
+ if (_argc == 2)
+ {
+ type = _argv[2];
+ }
+
+ ros::init(_argc, _argv, "optical_frame_publisher");
+
+ OpticalFramePublisher pub;
+ if (type == "depth")
+ pub.Init(false);
+ else
+ pub.Init();
+
+ ros::spin();
+
+ return 0;
+}