ros2 slower than ros1

edit

I am using a Pynq Z1 Board with a Zynq 7000 SoC. On my pc, I have a Ros Node running that publishes an sensor_msgs image. The Zynq receives this image and publishes it directly back to the PC. PC =>Image =>Zynq => Image => PC

When I am doing this with ROS1 I get roughly 25-30Hz, on ROS2 ~4Hz. I am using ROS1 melodic from the repository (Ubuntu 18.04) and compiled ROS2 foxy (Ubuntu 20.04) on the Zynq.

Are there some hidden copys in ros2 that i am not aware of? You can find the code for my nodes in the attachment.

As I don't have 5 karma, I can't put my code in the attachment. Therefore, i will post it here:

ROS1 PC Node

#include "ros/ros.h"
#include "sensor_msgs/Image.h"

#include <sstream>

#define COLS  640
#define ROWS  480

int main(int argc, char **argv)
{
  ros::init(argc, argv, "grayScale");
  ros::NodeHandle n;
  ros::Publisher img_pub = n.advertise<sensor_msgs::Image>("/usb_cam/image_raw", 1000);
  ros::Rate loop_rate(30);


  uint32_t count = 0;

  sensor_msgs::Image msg;

  msg.encoding ="rgb8";
  msg.height = COLS;
  msg.width = ROWS;
  msg.header.stamp = ros::Time::now();
  msg.header.seq = count;
  msg.header.frame_id = "frame_id";
  msg.step = COLS*3;  //RGB

  for(int i=0; i<COLS; i++) {
    for(int j=0; j<ROWS; j++) {
      uint8_t R=1;
      uint8_t G=2;
      uint8_t B=3;
      msg.data.push_back(R);
      msg.data.push_back(G);
      msg.data.push_back(B);
    }
  }

  while (ros::ok()) {

    img_pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}

ROS1 Zynq Node

#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include <sys/time.h>
#include <time.h>

extern "C" {
#include "pynq_api.h"
}

#define COLS  640
#define ROWS  480

#define R 0
#define G 1
#define B 2

#define samples 20

#define DMA_ADRESS 0x80400000


class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    PYNQ_allocatedSharedMemory(&shared_memory_1, ROWS*COLS*3, 1);
    PYNQ_allocatedSharedMemory(&shared_memory_2, ROWS*COLS*3, 1);
    d1=(__uint8_t*)shared_memory_1.pointer;
    d2=(__uint8_t*)shared_memory_2.pointer;
    output.encoding = "rgb8";
    output.step = 1*COLS;
    output.height = 0;
    output.width = 0;

    //Topic you want to publish
    pub_ = n_.advertise<sensor_msgs::Image>("/filteredHW", 100);

    //Topic you want to subscribe
    sub_ = n_.subscribe("/usb_cam/image_raw", 100, &SubscribeAndPublish::callback, this);
  }

  ~SubscribeAndPublish()
  {
    PYNQ_freeSharedMemory(&shared_memory_1);
    PYNQ_freeSharedMemory(&shared_memory_2);
  }

  void callback(const sensor_msgs::Image& input)
  {
      pub_.publish(input);
  }

private:
  ros::NodeHandle n_;
  ros::Publisher pub_;
  ros::Subscriber sub_;


  PYNQ_SHARED_MEMORY shared_memory_1, shared_memory_2;

  __uint8_t * d1;
  __uint8_t * d2;
  sensor_msgs::Image output;
  PYNQ_AXI_DMA dma;
};//End of class SubscribeAndPublish

int main(int argc, char **argv)
{
  //Initiate ROS
  ros::init(argc, argv, "subscribe_and_publish_grayScale");

  //Create an object of class SubscribeAndPublish that will take care of everything
  SubscribeAndPublish SAPObject;

  ros::spin();

  return 0;
}

ROS2 PC Node

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/image.hpp"

#define COLS  640
#define ROWS  480

using namespace std::chrono_literals;
using std::placeholders::_1;

class MinimalPubandSub : public rclcpp::Node
{
public:

  MinimalPubandSub()
  : Node("minimal_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<sensor_msgs::msg::Image>("topic", 30);
    subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
      "topic_back", 30, std::bind(&MinimalPubandSub::topic_callback, this, _1));
    timer_ = this->create_wall_timer(
      33ms, std::bind(&MinimalPubandSub ...