Getting polar_scan_matcher package working with published laser scanner message
I am running 2 nodes. One is psmnode (from polarscanmatcher package) and the other one is my own node (publishernode) that publishes scan messages.
When running those 2 nodes, I had a warning that I guess causing the rviz not visualizing as expected. The publisher is publishing messsages, however, in the rviz, from laser scan tab, it says "Showing [0] points from [0] messages". The warning is as below, from the terminal:
[ WARN] [1322334049.869100511]: ScanMatcherNode: Could get initial laser transform, skipping scan (Frame id /laser_frame does not exist! Frames (3): Frame /laser exists with parent /base_link.
Frame /base_link exists with parent NO_PARENT.
Anyway, the following is my launch file:
<launch>
<param name="/use_sim_time" value="false"/>
<node pkg="rviz" type="rviz" name="rviz"
args="-d $(find polar_scan_matcher)/demo/demo.vcg"/>
<!-- I include this, our own node -->
<node pkg="pcd_robot" type="publisher_node" name="publisher_node">
<remap from="/scan" to="scan"/>
</node>
<node pkg="polar_scan_matcher" type="psm_node" name="psm_node" output="screen">
<param name="max_error" value="0.20"/>
<param name="search_window" value="100"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
args="0.0 0.0 0.0 0 0 0 /base_link /laser 40" />
</launch>
Below is my code that is supposed to publish the laser scan messages(it's already working as a single node, but still cannot be used with psm_node):
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <fstream>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include <vector>
#include "laser_geometry/laser_geometry.h"
#include <tf/transform_listener.h>
#include <boost/algorithm/string.hpp>
#include "tf/tf.h"
#include "tf/transform_datatypes.h"
#include <tf/transform_broadcaster.h>
// PCL specific includes
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "pcl/common/angles.h"
using namespace std;
const int num_row = 264;
const int num_column = 1089;
class PointCloud
{
private:
ros::NodeHandle n;
// ros::Publisher chatter_pub;
ros::Publisher scan_pub;
ros::Publisher pose_pub;
int count;
vector<vector<double> > data_set;
public:
PointCloud();
~PointCloud();
/**
* Opens and reads file
*/
bool operateFile();
void publishLaser();
// void display(); //translate and publish
};
PointCloud::PointCloud()
{
scan_pub = n.advertise<sensor_msgs::LaserScan>("/scan", 50);
count = 0;
}
PointCloud::~PointCloud()
{
ROS_INFO("Shutting down publisher_node node.");
}
/**
* @brief Opens and reads file for further processing
*
* @param
*
* @return True if the file is successfully manipulated
*/
bool PointCloud::operateFile()
{
ifstream inFile; // object for reading from a file
ofstream outFile; // object for writing to a file
char inputFilename[] = "/home/alfa/ros_workspace/dataset1.txt";
inFile.open(inputFilename, ios::in);
if (!inFile) {
cerr << "Can't open input file " << inputFilename << endl;
exit(1);
}
else
cout << "The file is successfully opened :-)" << endl;
// Set up sizes. (num_row x num_column)
data_set.resize(num_row);
for (int i = 0; i < num_row; ++i)
data_set[i].resize(num_column);
// Store in a buffer/vector what have been read.
for (int j = 0; j < num_row; j++)
for (int k = 0; k < num_column; k++)
{
inFile >> data_set[j][k];
}
return (true);
}
void PointCloud::publishLaser()
{
int num_laser_readings;
double laser_frequency;
num_laser_readings = 1080;
laser_frequency = 40; //20 could be
ros::Rate r(1.0);
int t = 0;
while (n.ok() && (t < num_row))
{
sensor_msgs::LaserScan scan;
ros::Time scan_time( double(data_set[t][1]/1000));
for (int i = 0; i < num_row; i++)
{
scan.header.stamp = scan_time;
}
scan.header.frame_id = "laser_frame";
scan.angle_min = - 270 / 2 * M_PI / 180; // -xx degree
scan.angle_max = 270 / 2 * M_PI / 180; // xx degree
scan.angle_increment = (scan.angle_max-scan.angle_min)/num_laser_readings;
scan.time_increment = (1 / laser_frequency) / (num_laser_readings);
scan.range_min = 0.0;
scan.range_max = 100.0;
scan.set_ranges_size(num_laser_readings);
for(int i = 0; i < num_laser_readings; ++i)
{
// Laser scanner data reside in column 10th to 1080th
scan.ranges[i] = data_set[t][i+9] / 1000.0;
}
scan_pub.publish(scan);
++count;
++t;
r.sleep();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "publisher_node");
PointCloud pointCloud;
ROS_INFO("Node started");
ros::Rate loop_rate(10);
pointCloud.operateFile();
pointCloud.publishLaser();
ROS_INFO("Node finished");
return 0;
}
How do I fix this?
Asked by alfa_80 on 2011-12-02 07:28:05 UTC
Comments
@Ivan Dryanovski: Could you please answer some of my confusions regarding the laser_scan_matcher package tutorial here: http://answers.ros.org/question/3186/several-things-on-the-laser_scan_matcher-package..Thanks in advance..
Asked by alfa_80 on 2011-12-05 03:27:12 UTC
(2) In Laser Scan, under transforms, it says that "For frame [laser]: No transform to fixed frame [/world]. TF error: [Unable to lookup transform, cache is empty, when looking up transform from frame [/laser] to frame [/world]]"
Asked by alfa_80 on 2011-12-04 04:22:37 UTC
As I followed the setting in laser_scan_matcher demo, I got 2 errors in rviz: (1) In TF, under /laser frame, it complaints that "No transform from [/laser] to frame [/world]" "
Asked by alfa_80 on 2011-12-04 04:22:08 UTC
Try running the laser_scan_matcher demo, and look at how the tf's and topics look in rviz
Asked by Ivan Dryanovski on 2011-12-04 03:29:12 UTC
In rviz, instead of setting fixed frame as /world, can I set it to "/laser"? I've tried to set the fixed frame as /laser and target frame as "base_link", it works but still in the TF option, for /world, it complaints "No transform from [/world] to frame [/laser]". Having /world is important right?
Asked by alfa_80 on 2011-12-04 02:55:23 UTC
The strange thing also, the publisher is publishing messsages (after running "rostopic echo -c /scan" in the terminal), however, in the rviz, from laser scan tab, it says "Showing [0] points from [0] messages".
Asked by alfa_80 on 2011-12-04 02:28:20 UTC
Hopefully, you can guide me on this..thanks in advance.
Asked by alfa_80 on 2011-12-04 02:04:15 UTC
I've just tried laser_scan_matcher, however, I still got the similar error([ WARN] [1323010737.488732601]: ScanMatcher: Could not get initial laser transform(Frame id /laser_frame does not exist! Frames (3): Frame /laser exists with parent /base_link. Frame /base_link exists with parent NO_PARENT. )
Asked by alfa_80 on 2011-12-04 02:02:39 UTC
Have you tried using laser_scan_matcher? I moved my efforts to that one.
Asked by Ivan Dryanovski on 2011-12-03 06:27:33 UTC