Ask Your Question
0

Laser -> point cloud, tf drop all packages

asked 2014-05-25 17:04:01 -0500

dreamcase gravatar image

updated 2014-05-25 17:14:44 -0500

hi, all,

I am super new to ROS. I am trying to use tf to transform the laser input to tf base_link frame. But I am constantly getting error msg like "MessageFilter [target=base_link ]: Dropped 100.00% of messages so far. Please turn the [ros.agv.message_notifier] rosconsole logger to DEBUG for more information." and it seems the call back was never invoked. I tested the laser msg by subscribing to the raw msg and it works fine. I think there might be a broken link somewhere in the settings. Can someone help me? here's the code for the listener.

class LaserScanToPointCloud{
public:
  ros::NodeHandle node;
  laser_geometry::LaserProjection projector;
  tf::TransformListener listener;
  message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub;
  tf::MessageFilter<sensor_msgs::LaserScan> laser_notifier;
  ros::Publisher scan_pub;
  LaserScanToPointCloud(ros::NodeHandle n) :
    node(n),
    laser_sub(node, "base_scan", 10),
    laser_notifier(laser_sub,listener, "base_link", 10)
  {
      printf("setting up callback\r\n");
      laser_notifier.registerCallback(boost::bind(&LaserScanToPointCloud::scanCallback, this, _1));
      laser_notifier.setTolerance(ros::Duration(0.01));
      scan_pub = node.advertise<sensor_msgs::PointCloud>("my_cloud",1);
      printf("set up callback\r\n");
  }
  void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
  {
    sensor_msgs::PointCloud localcloud;
    printf("%ds:%lfm\r\n",scan_in->header.stamp.sec,scan_in->ranges[128]);
    try
    {
        projector.transformLaserScanToPointCloud("base_link",*scan_in, localcloud,listener);
    }
    catch (tf::TransformException& e)
    {
        std::cout << e.what();
        return;
    }
    printf("(%f,%f,%f)\r\n",localcloud.points.data()->x,localcloud.points.data()->y,localcloud.points.data()->z);
    // Do something with cloud.
    scan_pub.publish(localcloud);
  }
};
int main(int argc, char** argv)
{
  ros::init(argc, argv, "my_scan_to_cloud");
  ros::NodeHandle n;
  LaserScanToPointCloud lstopc(n);
  ros::spin();
  return 0;
}

and here's the broadcaster

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;
  ros::Rate r(50);
  tf::TransformBroadcaster broadcaster;
  while(n.ok()){
      broadcaster.sendTransform(
              tf::StampedTransform(
                      tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.0, 0.0, 0.2)),
                      ros::Time::now(),"base_link", "base_scan"));
      r.sleep();
  }
  return 0;
}

million thanks Ray

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-05-25 18:02:25 -0500

updated 2014-05-25 18:05:25 -0500

I am no 100% sure this could be the source of your problem, but on the broadcaster, after you call

r.sleep()

You should call too

ros::spinOnce();

So ROS can do its magic.

Anyway, if what you want is to translate a laser scan into a point cloud, don't re-invent the wheel, just take a look at laser_geometry package.

edit flag offensive delete link more

Comments

It doesn't work for me. I am trying to follow this one, http://wiki.ros.org/laser_pipeline/Tutorials/IntroductionToWorkingWithLaserScannerData the purpose is to take in laser scan and use TF to do the transform to get point cloud. btw, if I do "rostopic list", I am getting /base_scan /my_cloud /rosout /rosout_agg /tf /tf_static I don't even see the base_link topic. is it suppose so?

dreamcase gravatar imagedreamcase ( 2014-05-25 22:08:38 -0500 )edit

Hi, yes it is normal that you don't see base_link in the topic list, because it is not a topic. /base_link is part of the tf tree. What happens when you launch your nodes and run: rosrun tf tf_monitor /base_link /base_scan ?

Martin Peris gravatar imageMartin Peris ( 2014-05-26 15:02:19 -0500 )edit

I changed the naming a bit. it says "Frames: All Broadcasters: Node: /robot_tf_publisher 50.4358 Hz, Average Delay: 0.000220772 Max Delay: 0.000297859 RESULTS: for base_link to base_laser Chain is: base_link Net delay avg = 0.00950738: max = 0.020233 " if I do 'rostopic echo tf', I can see transforms: - header: seq: 0 stamp: secs: 1401285337 nsecs: 970860460 frame_id: base_link child_frame_id: base_laser transform: translation: x: 0.0 y: 0.0 z: 0.2 rotation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 But, on the listener node, I see nothing

dreamcase gravatar imagedreamcase ( 2014-05-28 03:57:41 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2014-05-25 17:04:01 -0500

Seen: 380 times

Last updated: May 25 '14