update rostopic dumper to new hdf5 interface

seerep_ros/seerep_ros_comm/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(SeerepCom REQUIRED)
 
 find_package(SeerepHdf5Pb REQUIRED)
 find_package(SeerepHdf5Fb REQUIRED)
+find_package(seerep_hdf5_ros REQUIRED)
 find_package(SeerepHdf5Core REQUIRED)
 find_package(SeerepCoreFb REQUIRED)
 find_package(SeerepCore REQUIRED)
@@ -58,6 +59,7 @@ catkin_package(
   roscpp
   seerep_ros_conversions_pb
   seerep_ros_conversions_fb
+  seerep_hdf5_ros
   roscpp
   topic_tools
   tf
@@ -71,6 +73,7 @@ include_directories(
   ${SeerepCom_INCLUDE_DIRS}
   ${SeerepHdf5Pb_INCLUDE_DIRS}
   ${SeerepHdf5Fb_INCLUDE_DIRS}
+  ${seerep_hdf5_ros_INCLUDE_DIRS}
   ${SeerepHdf5Core_INCLUDE_DIRS}
   ${SeerepCoreFb_INCLUDE_DIRS}
   ${seerep_ros_conversions_pb_INCLUDE_DIRS}
@@ -102,6 +105,7 @@ target_link_libraries(
   ${SeerepCom_LIBRARIES}
   ${seerep_ros_conversions_pb_LIBRARIES}
   ${SeerepHdf5Pb_LIBRARIES}
+  ${seerep_hdf5_ros_LIBRARIES}
   ${SeerepHdf5Core_LIBRARIES}
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}

seerep_ros/seerep_ros_comm/include/seerep_ros_comm/rostopic_dumper.h

@@ -1,59 +1,83 @@
-#ifndef SEEREP_ROS_ROSTOPIC_DUMPER_H_
-#define SEEREP_ROS_ROSTOPIC_DUMPER_H_
+#ifndef HDF5_NODE_H
+#define HDF5_NODE_H
 
 #include <ros/master.h>
 #include <ros/ros.h>
-#include <seerep_hdf5_core/hdf5_core_general.h>
-#include <seerep_hdf5_core/hdf5_core_image.h>
-#include <seerep_hdf5_pb/hdf5_pb_image.h>
-#include <seerep_hdf5_pb/hdf5_pb_pointcloud.h>
-#include <seerep_hdf5_pb/hdf5_pb_tf.h>
-#include <seerep_ros_conversions_pb/conversions.h>
-#include <tf2_msgs/TFMessage.h>
-
-#include <boost/functional/hash.hpp>
+#include <seerep_hdf5_ros/hdf5_ros.h>
+
 #include <boost/lexical_cast.hpp>
-#include <boost/uuid/uuid.hpp>
 #include <boost/uuid/uuid_generators.hpp>
-#include <boost/uuid/uuid_io.hpp>
-#include <functional>
+#include <chrono>
 #include <optional>
+#include <thread>
 
-#include "types.h"
-
-namespace seerep_ros_comm
+namespace seerep_ros_examples
 {
-class DumpSensorMsgs
+
+std::string getCameraInfoTopic(const std::string& imageTopic);
+bool topicAvailable(const std::string& topic, ros::master::TopicInfo& info);
+
+class Hdf5Node
 {
 public:
-  DumpSensorMsgs(std::string hdf5FilePath, std::string project_frame_id,
-                 std::string project_name);
+  Hdf5Node() = delete;
 
-  std::optional<ros::Subscriber> getSubscriber(const std::string& message_type,
-                                               const std::string& topic);
+  Hdf5Node(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
 
-  void dump(const std_msgs::Header::ConstPtr& msg) const;
+  void subscribe();
 
-  void dump(const sensor_msgs::PointCloud2::ConstPtr& msg) const;
+private:
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
 
-  void dump(const sensor_msgs::Image::ConstPtr& msg) const;
+  /* store the topics to dump */
+  std::vector<std::string> topics_;
 
-  void dump(const geometry_msgs::Point::ConstPtr& msg) const;
+  /* keeps the subsribers alive outside of their initial function scope */
+  std::map<std::string, ros::Subscriber> subscribers_;
 
-  void dump(const geometry_msgs::Quaternion::ConstPtr& msg) const;
+  /*
+   * Current workaround is to store the first camera info message and use it
+   * for all following images. If the camera info is  subject to change, the
+   * camera and camera info topic should be time synchronized.
+   * This could be achieved by using message filters.
+   * http://wiki.ros.org/message_filters
+   */
+  std::string cameraInfoUuid_;
 
-  void dump(const tf2_msgs::TFMessage::ConstPtr& msg) const;
+  /* composition to the seerep_hdf5_ros interface */
+  std::unique_ptr<seerep_hdf5_ros::Hdf5Ros> hdf5Ros_;
 
-private:
-  std::vector<seerep_core_msgs::LabelCategory> m_labelsCategory;
+  /* @brief Get a parameter from the parameter server.
+   *
+   * @tparam T Type of the parameter.
+   * @param name Nname of the parameter.
+   * @return Vvalue of the parameter.
+   * @throws std::runtime_error if the parameter is missing.
+   */
+  template <typename T>
+  T getParameter(const std::string& name);
 
-  std::shared_ptr<seerep_hdf5_pb::Hdf5PbTf> m_ioTf;
-  std::shared_ptr<seerep_hdf5_pb::Hdf5PbPointCloud> m_ioPointCloud;
-  std::shared_ptr<seerep_hdf5_pb::Hdf5PbImage> m_ioImage;
-  std::shared_ptr<seerep_hdf5_core::Hdf5CoreImage> m_ioImageCore;
-  ros::NodeHandle nh;
+  /*
+    Callbacks for the subscribers, some require additional logic, thus it is
+    not possible to unify them with a template.
+  */
+  void callback(const sensor_msgs::Image::ConstPtr& msg);
+  void callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
+  void callback(const tf2_msgs::TFMessage::ConstPtr& msg);
 };
 
-}  // namespace seerep_ros_comm
+template <typename T>
+T Hdf5Node::getParameter(const std::string& name)
+{
+  T value;
+  if (!pnh_.getParam(name, value))
+  {
+    throw std::runtime_error("Paramter '" + name + "' is missing");
+  }
+  return value;
+}
+
+} /* Namespace seerep_ros_examples */
 
-#endif  // SEEREP_ROS_ROSTOPIC_DUMPER_H_
+#endif /* HDF5_NODE_H_ */

seerep_ros/seerep_ros_comm/launch/rostopic_dumper.launch

@@ -1,13 +1,15 @@
 <?xml version="1.0" encoding="utf-8" ?>
 <launch>
-    <arg name="topics" default="['/image', '/tf', '/point_cloud2']"/>
-    <arg name="hdf5FolderPath" default="/seerep/seerep-data/"/>
-    <arg name="project_frame_id" default="map"/>
+    <arg name="topics" default="['/camera_r/depth/image_rect_raw', '/tf']"/>
+    <arg name="path" default="/seerep/seerep-data/"/>
+    <arg name="project_root_frame" default="map"/>
     <arg name="project_name" default="testproject"/>
-    <node name="seerep_ros_communication_rostopic_dumper" pkg="seerep_ros_comm" type="rostopic_dumper" output="screen">
+    <arg name="max_viewing_distance" default="1.0"/>
+    <node name="seerep_ros_examples_hdf5_node" pkg="seerep_ros_examples" type="seerep_ros_examples_hdf5_node" output="screen">
         <rosparam param="topics" subst_value="True">$(arg topics)</rosparam>
-        <rosparam param="hdf5FolderPath" subst_value="True">$(arg hdf5FolderPath)</rosparam>
-        <rosparam param="project_frame_id" subst_value="True">$(arg project_frame_id)</rosparam>
+        <rosparam param="path" subst_value="True">$(arg path)</rosparam>
+        <rosparam param="project_root_frame" subst_value="True">$(arg project_root_frame)</rosparam>
         <rosparam param="project_name" subst_value="True">$(arg project_name)</rosparam>
+        <rosparam param="max_viewing_distance" subst_value="True">$(arg max_viewing_distance)</rosparam>
     </node>
 </launch>

seerep_ros/seerep_ros_comm/package.xml

@@ -18,6 +18,7 @@
   <depend>seerep_core_fb</depend>
   <depend>seerep_hdf5_pb</depend>
   <depend>seerep_hdf5_fb</depend>
+  <depend>seerep_hdf5_ros</depend>
   <depend>seerep_hdf5_core</depend>
   <depend>seerep_ros_conversions_fb</depend>
   <depend>seerep_ros_conversions_pb</depend>