rosbagger: switches to boost::shared_ptr for handling instances of

rosbag::Bag (fixes #28 and resolves #29)
pal-robotics · Mar 27, 2014 · 2c62db8 · 2c62db8
1 parent 9796f4b
commit 2c62db8
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 22 deletions.
diff --git a/motion_retargeting/include/motion_retargeting/motion_recorder/rosbagger.h b/motion_retargeting/include/motion_retargeting/motion_recorder/rosbagger.h
@@ -48,7 +48,7 @@ class Rosbagger : public MotionRecorder
   ~Rosbagger()
   {
     // Just to make sure the bag is closed
-    bag_.close();
+    bag_ptr_->close();
   };
 
   virtual bool prepareRecording()
@@ -60,13 +60,14 @@ class Rosbagger : public MotionRecorder
       ss >> bag_name_;
       try
       {
-        bag_.open(bag_name_, rosbag::bagmode::Write);
+        bag_ptr_ = boost::shared_ptr<rosbag::Bag>(new rosbag::Bag(bag_name_, rosbag::bagmode::Write));
         ROS_INFO_STREAM("Rosbagger: Ready to write to rosbag '" << bag_name_ << "'.");
         recording_ = true;
       }
       catch (rosbag::BagException& e)
       {
-        ROS_ERROR_STREAM("Rosbagger: Exception thrown while creating rosbag '" << bag_name_ << "'!");
+        ROS_ERROR_STREAM("Rosbagger: Exception thrown while creating rosbag '" << bag_name_ << "'!"
+                         << "Error: " << e.what());
         throw std::invalid_argument(bag_name_);
       }
       return true;
@@ -82,7 +83,7 @@ class Rosbagger : public MotionRecorder
   {
     if (recording_)
     {
-      bag_.close();
+      bag_ptr_->close();
       recording_ = false;
       ROS_INFO_STREAM("Rosbagger: Rosbag '" << bag_name_ << "' closed.");
       return true;
@@ -100,7 +101,7 @@ class Rosbagger : public MotionRecorder
     {
       try
       {
-        bag_.write(recorded_motion_topic_name_, ros::Time::now(), joint_states);
+        bag_ptr_->write(recorded_motion_topic_name_, ros::Time::now(), joint_states);
         return true;
       }
       catch (rosbag::BagIOException& e)
@@ -126,15 +127,16 @@ class Rosbagger : public MotionRecorder
       {
         try
         {
-          bag_.open(bag_name_, rosbag::bagmode::Read);
+          bag_ptr_ = boost::shared_ptr<rosbag::Bag>(new rosbag::Bag(bag_name_, rosbag::bagmode::Read));
         }
         catch (rosbag::BagException& e)
         {
-          ROS_ERROR_STREAM("Rosbagger: Exception thrown while opening rosbag '" << bag_name_ << "'!");
+          ROS_ERROR_STREAM("Rosbagger: Exception thrown while opening rosbag '" << bag_name_ << "'!"
+                           << "Error: " << e.what());
           throw std::invalid_argument(bag_name_);
           return false;
         }
-        view_ptr_ = boost::shared_ptr<rosbag::View>(new rosbag::View(bag_,
+        view_ptr_ = boost::shared_ptr<rosbag::View>(new rosbag::View(*bag_ptr_,
                                                                      rosbag::TopicQuery(recorded_motion_topic_name_)));
         view_it_ = view_ptr_->begin();
         ROS_INFO_STREAM("Rosbagger: Ready to read from rosbag '" << bag_name_ << "'.");
@@ -160,7 +162,7 @@ class Rosbagger : public MotionRecorder
     if (playback_)
     {
       playback_ = false;
-      bag_.close();
+      bag_ptr_->close();
       ROS_INFO_STREAM("Rosbagger: Rosbag '" << bag_name_ << "' closed.");
       return true;
     }
@@ -175,23 +177,32 @@ class Rosbagger : public MotionRecorder
   {
     if (playback_)
     {
-      if (view_it_ != view_ptr_->end())
+      while (view_it_ != view_ptr_->end())
       {
-        sensor_msgs::JointState::ConstPtr js_ptr = view_it_->instantiate<sensor_msgs::JointState>();
-        if (js_ptr != NULL)
+        sensor_msgs::JointState::ConstPtr js_ptr;
+        try
         {
-          joint_states = *js_ptr;
-          ROS_DEBUG_STREAM("Rosbagger: Retrieved this joint state message from the rosbag: "
-                           << joint_states);
-          view_it_++;
-          return true;
+          js_ptr = view_it_->instantiate<sensor_msgs::JointState>();
+          if (js_ptr != NULL)
+          {
+            joint_states = *js_ptr;
+            ROS_DEBUG_STREAM("Rosbagger: Retrieved this joint state message from the rosbag: "
+                             << joint_states);
+            view_it_++;
+            return true;
+          }
+          else
+          {
+            ROS_WARN_STREAM("Rosbagger: Couldn't retrieve joint state message from rosbag!");
+          }
         }
-        else
+        catch (rosbag::BagFormatException& e)
         {
-          ROS_WARN_STREAM("Rosbagger: Couldn't retrieve joint state message from rosbag!");
-          return false;
+          ROS_WARN_STREAM("Couldn't read message from bag. Error: " << e.what());
         }
+        view_it_++;
       }
+
       ROS_INFO_STREAM("Rosbagger: We reached the end of the rosbag.");
       return false;
     }
@@ -206,7 +217,7 @@ class Rosbagger : public MotionRecorder
   /**
    * The rosbag to write data to/read data from
    */
-  rosbag::Bag bag_;
+  boost::shared_ptr<rosbag::Bag> bag_ptr_;
   /**
    * Pointer to a rosbag view for reading from the rosbag
    */

diff --git a/motion_retargeting/src/motion_player_rosbag_fjta.cpp b/motion_retargeting/src/motion_player_rosbag_fjta.cpp
@@ -196,6 +196,7 @@ int main (int argc, char** argv)
           playback_status_msg.status_message =
               motion_retargeting_msgs::errorCodeToString(playback_status_msg.status_code);
           pub_playback_status.publish(playback_status_msg);
+          ROS_INFO_STREAM("Playing back motion. [" << node_name << "]");
           status_message_published = true;
         }
         if(rosbagger.readMotion(joint_states))
@@ -213,7 +214,7 @@ int main (int argc, char** argv)
           pub_count++;
           time_step.sleep();
           output_handler.setOutput(joint_states);
-          ROS_INFO_STREAM_THROTTLE(1.0, "Current/Average publishing frequency: " << (1/time_step.toSec()) << " / "
+          ROS_DEBUG_STREAM_THROTTLE(1.0, "Current/Average publishing frequency: " << (1/time_step.toSec()) << " / "
                                    << avg_freq << " [" << node_name << "]");
         }
         else