How to deliver arguments to a callback function?

[zlwnge]

Hi, @staff

I’m caught in an error when I try to deliver arguments to my callback function.

I find a method in ROS Answer.But I also encountered a bug when applying this method to my problem.

There is the key line:

map_sub_ = n.subscribe<nav_msgs::OccupancyGrid>("/map", 100, boost::bind(&MapGenerator::mapCallback, this, _1, _2));

void mapCallback(const nav_msgs::OccupancyGridConstPtr& map, const nav_msgs::OdometryConstPtr& odom){}

And there is my all source code, as you see, I just modified a map_saver.cpp file in map_server package:

#include <cstdio>
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/GetMap.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"
#include "nav_msgs/Odometry.h"
#include "nav_msgs/OccupancyGrid.h"
// #include "message_filters/subscriber.h"
// #include "message_filters/synchronizer.h"
// #include "message_filters/sync_policies/approximate_time.h"

using namespace std;

/**
 * @brief Map generation node.
 */
class MapGenerator
{

  public:

    std::string mapname_;
    ros::Subscriber map_sub_;
    ros::Subscriber imu_sub_;
    bool saved_map_;
    int threshold_occupied_;
    int threshold_free_;


    MapGenerator(const std::string& mapname, int threshold_occupied, int threshold_free)
      : mapname_(mapname), saved_map_(false), threshold_occupied_(threshold_occupied), threshold_free_(threshold_free)
    {
      ros::NodeHandle n;
      ROS_INFO("Waiting for the map");
      imu_sub_ = n.subscribe<nav_msgs::Odometry>("/odom", 100, this);
      map_sub_ = n.subscribe<nav_msgs::OccupancyGrid>("/map", 100, boost::bind(&MapGenerator::mapCallback, this, _1, _2));

      // message_filters::Subscriber<nav_msgs::OccupancyGrid> map_sub(n, "map", 1);
      // message_filters::Subscriber<nav_msgs::Odometry> imu_sub(n, "odom", 1);

      // typedef message_filters::sync_policies::ApproximateTime<nav_msgs::OccupancyGrid, nav_msgs::Odometry> MySyncPolicy;
      // message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), imu_sub, map_sub);
      // sync.registerCallback(boost::bind(&MapGenerator::mapCallback, this, _1, _2));


      // ros::spin();
    }

    void mapCallback(const nav_msgs::OccupancyGridConstPtr& map, const nav_msgs::OdometryConstPtr& odom)
    {
      ROS_INFO("Received a %d X %d map @ %.3f m/pix",
               map->info.width,
               map->info.height,
               map->info.resolution);


      std::string mapdatafile = mapname_ + ".pgm";
      ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
      FILE* out = fopen(mapdatafile.c_str(), "w");
      if (!out)
      {
        ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
        return;
      }

      fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
              map->info.resolution, map->info.width, map->info.height);
      for(unsigned int y = 0; y < map->info.height; y++) {
        for(unsigned int x = 0; x < map->info.width; x++) {
          unsigned int i = x + (map->info.height - y - 1) * map->info.width;
          if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
            fputc(254, out);
          } else if (map->data[i] >= threshold_occupied_) { // (occ,255]
            fputc(000, out);
          } else { //occ [0.25,0.65]
            fputc(205, out);
          }
        }
      }

      fclose(out);


      std::string mapmetadatafile = mapname_ + ".yaml";
      ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str());
      FILE* yaml = fopen(mapmetadatafile.c_str(), "w");

      //get origin pose
      geometry_msgs::Quaternion orientation = map->info.origin.orientation;
      tf2::Matrix3x3 mat(tf2::Quaternion(
        orientation.x,
        orientation.y,
        orientation.z,
        orientation.w
      ));
      double yaw, pitch, roll;
      mat.getEulerYPR(yaw, pitch, roll);


      //get end pose
      tf2_ros::Buffer tfBuffer;
      tf2_ros::TransformListener tfListener(tfBuffer);
      geometry_msgs::Quaternion rot;
      ros::Time now = ros::Time::now();
      try{
        // tfBuffer.canTransform("/map", "/odom", now, ros::Duration(1.0));
        rot = tfBuffer.lookupTransform("/map", "/odom", now).transform.rotation;
        // ROS_INFO(rot);
      }catch(tf2::TransformException &ex){
        ROS_ERROR("lookupTransform Error: %s", ex.what());
      }
      // the incoming geometry_msgs::Quaternion is transformed to a tf2::Quaterion
      tf2::Matrix3x3 mat_end(tf2::Quaternion(
        rot.x,
        rot.y,
        rot.z,
        rot.w
      ));

      // the tf::Quaternion has a method to acess roll pitch and yaw
      double roll_end, pitch_end, yaw_end;
      mat_end.getEulerYPR(yaw_end, pitch_end, roll_end);


      fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nend: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
              mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw, odom->pose.pose.position.x, odom->pose.pose.position.y, yaw_end);
      // fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
      //         mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw);

      fclose(yaml);

      ROS_INFO("Done\n");
      saved_map_ = true;
    }

};


#define USAGE "Usage: \n" \
              "  map_saver -h\n"\
              "  map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] [ROS remapping args]"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "map_saver");
  std::string mapname = "map";
  int threshold_occupied = 65;
  int threshold_free = 25;

  for(int i=1; i<argc; i++)
  {
    if(!strcmp(argv[i], "-h"))
    {
      puts(USAGE);
      return 0;
    }
    else if(!strcmp(argv[i], "-f"))
    {
      if(++i < argc)
        mapname = argv[i];
      else
      {
        puts(USAGE);
        return 1;
      }
    }
    else if (!strcmp(argv[i], "--occ"))
    {
      if (++i < argc)
      {
        threshold_occupied = std::atoi(argv[i]);
        if (threshold_occupied < 1 || threshold_occupied > 100)
        {
          ROS_ERROR("threshold_occupied must be between 1 and 100");
          return 1;
        }

      }
      else
      {
        puts(USAGE);
        return 1;
      }
    }
    else if (!strcmp(argv[i], "--free"))
    {
      if (++i < argc)
      {
        threshold_free = std::atoi(argv[i]);
        if (threshold_free < 0 || threshold_free > 100)
        {
          ROS_ERROR("threshold_free must be between 0 and 100");
          return 1;
        }

      }
      else
      {
        puts(USAGE);
        return 1;
      }
    }
    else
    {
      puts(USAGE);
      return 1;
    }
  }

  if (threshold_occupied <= threshold_free)
  {
    ROS_ERROR("threshold_free must be smaller than threshold_occupied");
    return 1;
  }

  MapGenerator mg(mapname, threshold_occupied, threshold_free);

  while(!mg.saved_map_ && ros::ok())
    ros::spinOnce();

  return 0;
}

Finally, there is the error info:

I guess this error is caused by my mistake of using boost::bind() when delivering arguments. If you have a better method, please let me know.

Thank you so much!

[rzegers]

Hi @zlwnge,

I am not sure what specific changes you made to the a map_saver.cpp file inside the map_server package since I don’t have a diff view that will show the lines added/changed/removed. However, in general, I think the easiest way of accessing data inside callback function is to add a new class member variable to the MapGenerator class shown above and access that class member variable from inside the callback function (defined as a class member function) directly. Have you tried this approach?

Cheers,

Roberto

[zlwnge]

Hi @rzegers ,

Thank you for your reply, first.

And I tried this approach. I always get errors when calling the callback function. As you can see, I have to deliver the subscribed /odom data and /map data into the callback function, which makes me error, otherwise, I would not be able to manipulate this data.

zlwnge