Hey all, I am trying to convert an incoming laser scan from a hokuyo urg laser scanner into a pointcloud for the purpose of building a octomap. Here is the code I'm working with, which is mostly from a tutorial:
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, "scan", 10),
laser_notifier(laser_sub, listener, "Brain5", 10) {
laser_notifier.registerCallback(boost::bind(&LaserScanToPointCloud::scanCallback, this, _1));
laser_notifier.setTolerance(ros::Duration(.01));
scan_pub = node.advertise<sensor_msgs::PointCloud2>("my_cloud", 1);
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in) {
sensor_msgs::PointCloud2 cloud;
try {
projector.transformLaserScanToPointCloud("Brain5", *scan_in, cloud, listener);
}
catch(tf::TransformException& e) {
std::cout << e.what();
return;
}
std::cout << "publishing" << std::endl;
scan_pub.publish(cloud);
/* tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(0, 0, 0));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "odom_combined"));*/
}
};
int main(int argc, char** argv) {
ros::init(argc, argv, "my_scan_to_cloud");
ros::NodeHandle n;
LaserScanToPointCloud lstopc(n);
ros::spin();
return 0;
}
The code works fine when the tf frame that I pass to the Message Filter is the same as the laser's frame. But I have issues when I try to add transforms from the laser frame (not fixed) to a fixed frame. When I add this extra frame, the code will sporadically spit out a pointcloud, but no where near the consistency when I just use one frame.
I get this warning every time I run the code, if it's important:
[ WARN] [1376004701.242796255]: Message from [/hokuyo_node] has a non-fully-qualified frame_id [laser]. Resolved locally to [/laser]. This is will likely not work in multi-robot systems. This message will only print once.
Thanks in advance.
Originally posted by Challen on ROS Answers with karma: 11 on 2013-08-08
Post score: 0