ros-perception / image_common

Common code for working with images in ROS
http://www.ros.org/wiki/image_common
129 stars 223 forks source link

[ros2] image_transport subscription callback not called #138

Open zmk5 opened 5 years ago

zmk5 commented 5 years ago

Hi!

I've been working on a project using image_transport to publish maps of a region, but I can't seem to get the image_transportsubscription callback to work. The following is a snippet of the code I used:

Robot::Robot(uint robot_id_, std::string robot_name_, NS_my_planner::base_planner* planner_, uint no_of_robots,
             double radial_noise,
             std::string nbh_service_name,
             double sensing_radius_):
    Node("robot_node"),
    robot_id{robot_id_},
    robot_name{robot_name_},
    planner{planner_},
    abs_pose{0.0, 0.0, 0.0},
    sensing_radius{sensing_radius_},
    last_communication(no_of_robots),
    it_{rclcpp::Node::make_shared("image_node")},
    avoidCount{0},
    randCount{0}

/**
 * Constructor for the robot class
 * @param robot_id_ : the id number of the robot
 * @param robot_name_ : the common name for the robot
 * @param planner_ : the planner object pointer
 * @param radial_noise : the variance of the radial noise of the range sensor
 * @param sensing_radius_ : the sensing radius of the robot
 */
{
  std:: string robot_name_aug = robot_name_ + std::to_string(robot_id_);
  laser_sensor.range_noise_const = radial_noise;

  // Setting up the data path for the robot
  data_path = data_path + std::to_string(robot_id_) + "/";

  // Setting up the subscriber to laser ranger sensors
  sub_laser_scan_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
    "scan", std::bind(&Robot::laser_scanner_callback, this, std::placeholders::_1), rmw_qos_profile_sensor_data);

  // Setting up the subscriber to error free gps pose
  sub_abs_pose_ = this->create_subscription<nav_msgs::msg::Odometry>(
    "base_pose_ground_truth", 10, std::bind(&Robot::base_pose_ground_truth_callback, this, std::placeholders::_1));

  // Setting up the publisher to sent maps stored in the robot
  pub_map_ = it_.advertise("map", 1);

  // Setting up the subscriber to get the map of the robots
  for(uint i=0; i<no_of_robots; i++)
  {
    if(i != robot_id_)
    {
      sub_map_[i] = it_.subscribe(
          "/tb3_" + std::to_string(i) + "/map", 1,
          std::bind(&Robot::update_map_callback, this, std::placeholders::_1, i));
    }
  }

  // Setting up the publisher to command velocity
  pub_cmd_vel_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);

  // Setting up the client for neighbor service
  get_nbh_client_ = this->create_client<levy_walk_msgs::srv::Neighbors>(nbh_service_name);

  // Set up timer
  timer_ = this->create_wall_timer(500ms, std::bind(&Robot::timer_callback, this));
  static_time = this->now().seconds();
  pre_time = 0;
  count = 0;

}

I am currently running this code on Ubuntu 18.04 running ros2 Dashing. I suspect this might be an issue with the fact that I am running do separate nodes within the same class. The image_transport publisher works and I can read what it's publishing using ros2 topic echo /tb3_0/map.

Running ros2 topic info /tb3_0/map shows that the publisher and the subscriptions are registering and seeing each other.

Finally, I used two methods within the image_transport publisher and subscriber class (getNumSubscribers() and getNumPublishers()) to see how many subscriptions and publishers are seen by the publisher and subscribers, respectively. Both returned the correct number of subscribers and publishers seen.