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 laser_sub;
tf::MessageFilter 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("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
↧