Skip to content

Commit

Permalink
divide examples into two cases: with and without deserialisation
Browse files Browse the repository at this point in the history
  • Loading branch information
sangteak601 committed Mar 16, 2024
1 parent 6cfd645 commit 7da7fed
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 10 deletions.
23 changes: 16 additions & 7 deletions rosbag2_examples/rosbag2_examples_cpp/src/simple_bag_player.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,13 +51,20 @@ class SimpleBagPlayer : public rclcpp::Node
continue;
}

rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
std_msgs::msg::String::SharedPtr ros_msg = std::make_shared<std_msgs::msg::String>();

serialization_.deserialize_message(&serialized_msg, ros_msg.get());

publisher_->publish(*ros_msg);
std::cout << ros_msg->data << "\n";
if (message_needs_to_be_edit_before_send_)
{
rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
std_msgs::msg::String::SharedPtr ros_msg = std::make_shared<std_msgs::msg::String>();

serialization_.deserialize_message(&serialized_msg, ros_msg.get());
ros_msg->data += "[edited]";
publisher_->publish(*ros_msg);
std::cout << ros_msg->data << "\n";
}
else
{
publisher_->publish(*msg->serialized_data);
}

break;
}
Expand All @@ -68,6 +75,8 @@ class SimpleBagPlayer : public rclcpp::Node

rclcpp::Serialization<std_msgs::msg::String> serialization_;
rosbag2_cpp::Reader reader_;

bool message_needs_to_be_edit_before_send_ {true};
};

int main(int argc, char ** argv)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,15 +34,21 @@ def __init__(self, bag_filename):
self.publisher = self.create_publisher(String, 'chatter', 10)
# ignore timestamp and publish at a fixed rate (10 Hz).
self.timer = self.create_timer(0.1, self.timer_callback)
self.message_needs_to_be_edit_before_send_ = True

def timer_callback(self):
while self.reader.has_next():
msg = self.reader.read_next()
if msg[0] != 'chatter':
continue
ros_msg = deserialize_message(msg[1], String)
self.publisher.publish(ros_msg)
self.get_logger().info(ros_msg.data)

if self.message_needs_to_be_edit_before_send_:
ros_msg = deserialize_message(msg[1], String)
ros_msg.data += '[edited]'
self.publisher.publish(ros_msg)
self.get_logger().info(ros_msg.data)
else:
self.publisher.publish(msg[1])


def main(args=None):
Expand Down

0 comments on commit 7da7fed

Please sign in to comment.