Robotics StackExchange | Archived questions

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

Answers