Merge branch 'tim-fan-master'

appliedAI-Initiative · Apr 22, 2020 · 9207b77 · 9207b77
2 parents 9da9035 + 5a8f5b7
commit 9207b77
Show file tree

Hide file tree

Showing 7 changed files with 39 additions and 23 deletions.
diff --git a/README.md b/README.md
@@ -106,6 +106,7 @@ The static parameters are send to the ROS parameter server at startup and are no
 - **publish_pose**: Bool. If a PoseStamped message should be published. Even if this is false the tf will still be published.
 - **pointcloud_frame_id**: String. The Frame id of the Pointcloud/map.
 - **camera_frame_id**: String. The Frame id of the camera position.
+- **load_calibration_from_cam**: Bool. If true, camera calibration is read from a `camera_info` topic. Otherwise it is read from launch file params.
 
 Dynamic parameters can be changed at runtime. Either by updating them directly via the command line or by using [rqt_reconfigure](http://wiki.ros.org/rqt_reconfigure) which is the recommended way.
 The parameters are:
@@ -125,13 +126,19 @@ The following topics are being published and subscribed to by the nodes:
 - A **tf** from the pointcloud frame id to the camera frame id (the position).
 
 ### Subscribed topics
-- The mono node subscribes to **/camera/image_raw** for the input image.
-
-- The RGBD node subscribes to **/camera/rgb/image_raw** for the RGB image and
-- **/camera/depth_registered/image_raw** for the depth information.
-
-- The stereo node subscribes to **image_left/image_color_rect** and
-- **image_right/image_color_rect** for corresponding images.
+- The mono node subscribes to:
+    - **/camera/image_raw** for the input image
+    - **/camera/camera_info** for camera calibration (if `load_calibration_from_cam`) is `true`
+
+- The RGBD node subscribes to:
+    - **/camera/rgb/image_raw** for the RGB image
+    - **/camera/depth_registered/image_raw** for the depth information
+    - **/camera/rgb/camera_info** for camera calibration (if `load_calibration_from_cam`) is `true`
+
+- The stereo node subscribes to:
+    - **image_left/image_color_rect** and
+    - **image_right/image_color_rect** for corresponding images
+    - **image_left/camera_info** for camera calibration (if `load_calibration_from_cam`) is `true`
 
 # 4. Services
 All nodes offer the possibility to save the map via the service node_type/save_map.

diff --git a/ros/include/Node.h b/ros/include/Node.h
@@ -51,6 +51,7 @@ class Node
   public:
     Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport);
     ~Node ();
+    void Init ();
 
   protected:
     void Update ();
@@ -81,6 +82,7 @@ class Node
 
     std::string name_of_node_;
     ros::NodeHandle node_handle_;
+    image_transport::ImageTransport image_transport_;
 
     ORB_SLAM2::System::eSensor sensor_;
 

diff --git a/ros/launch/orb_slam2_d435_mono.launch b/ros/launch/orb_slam2_d435_mono.launch
@@ -12,7 +12,6 @@
        <!-- static parameters -->
        <param name="load_map" type="bool" value="false" />
        <param name="map_file" type="string" value="map.bin" />
-       <param name="settings_file" type="string" value="$(find orb_slam2_ros)/orb_slam2/config/RealSenseD435Mono.yaml" />
        <param name="voc_file" type="string" value="$(find orb_slam2_ros)/orb_slam2/Vocabulary/ORBvoc.txt" />
 
        <param name="pointcloud_frame_id" type="string" value="map" />

diff --git a/ros/src/MonoNode.cc b/ros/src/MonoNode.cc
@@ -15,6 +15,8 @@ int main(int argc, char **argv)
 
     MonoNode node (ORB_SLAM2::System::MONOCULAR, node_handle, image_transport);
 
+    node.Init();
+
     ros::spin();
 
     ros::shutdown();

diff --git a/ros/src/Node.cc b/ros/src/Node.cc
@@ -2,12 +2,25 @@
 
 #include <iostream>
 
-Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) {
+Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, image_transport::ImageTransport &image_transport) :  image_transport_(image_transport) {
   name_of_node_ = ros::this_node::getName();
   node_handle_ = node_handle;
   min_observations_per_point_ = 2;
   sensor_ = sensor;
+}
+
+
+Node::~Node () {
+  // Stop all threads
+  orb_slam_->Shutdown();
+
+  // Save camera trajectory
+  orb_slam_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
+
+  delete orb_slam_;
+}
 
+void Node::Init () {
   //static parameters
   node_handle_.param(name_of_node_+ "/publish_pointcloud", publish_pointcloud_param_, true);
   node_handle_.param(name_of_node_+ "/publish_pose", publish_pose_param_, true);
@@ -22,7 +35,7 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima
    ORB_SLAM2::ORBParameters parameters;
    LoadOrbParameters (parameters);
 
-  orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, sensor, parameters, map_file_name_param_, load_map_param_);
+  orb_slam_ = new ORB_SLAM2::System (voc_file_name_param_, sensor_, parameters, map_file_name_param_, load_map_param_);
 
   service_server_ = node_handle_.advertiseService(name_of_node_+"/save_map", &Node::SaveMapSrv, this);
 
@@ -31,7 +44,7 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima
   dynamic_param_callback = boost::bind(&Node::ParamsChangedCallback, this, _1, _2);
   dynamic_param_server_.setCallback(dynamic_param_callback);
 
-  rendered_image_publisher_ = image_transport.advertise (name_of_node_+"/debug_image", 1);
+  rendered_image_publisher_ = image_transport_.advertise (name_of_node_+"/debug_image", 1);
   if (publish_pointcloud_param_) {
     map_points_publisher_ = node_handle_.advertise<sensor_msgs::PointCloud2> (name_of_node_+"/map_points", 1);
   }
@@ -40,18 +53,6 @@ Node::Node (ORB_SLAM2::System::eSensor sensor, ros::NodeHandle &node_handle, ima
   if (publish_pose_param_) {
     pose_publisher_ = node_handle_.advertise<geometry_msgs::PoseStamped> (name_of_node_+"/pose", 1);
   }
-
-}
-
-
-Node::~Node () {
-  // Stop all threads
-  orb_slam_->Shutdown();
-
-  // Save camera trajectory
-  orb_slam_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
-
-  delete orb_slam_;
 }
 
 
@@ -235,6 +236,7 @@ void Node::LoadOrbParameters (ORB_SLAM2::ORBParameters& parameters) {
   }
 
   if (load_calibration_from_cam) {
+    ROS_INFO_STREAM ("Listening for camera info on topic " << node_handle_.resolveName(camera_info_topic_));
     sensor_msgs::CameraInfo::ConstPtr camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic_, ros::Duration(1000.0));
     if(camera_info == nullptr){
         ROS_WARN("Did not receive camera info before timeout, defaulting to launch file params.");

diff --git a/ros/src/RGBDNode.cc b/ros/src/RGBDNode.cc
@@ -16,6 +16,8 @@ int main(int argc, char **argv)
 
     RGBDNode node (ORB_SLAM2::System::RGBD, node_handle, image_transport);
 
+    node.Init();
+
     ros::spin();
 
     ros::shutdown();

diff --git a/ros/src/StereoNode.cc b/ros/src/StereoNode.cc
@@ -19,6 +19,8 @@ int main(int argc, char **argv)
     // initialize
     StereoNode node (ORB_SLAM2::System::STEREO, node_handle, image_transport);
 
+    node.Init();
+
     ros::spin();
 
     return 0;