Ask Your Question
1

How to transform a laserscan message with TF?

asked 2013-12-19 04:18:50 -0600

AlphaOne gravatar image

updated 2013-12-19 07:02:41 -0600

I am trying to transform a laserscan message from one frame to another. The purpose of this is to then merge the resulting laserscans into a single scan. So far I have the following code:

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <math.h>
#include <sstream>
#include <time.h>
#include <geometry_msgs/PointStamped.h>

std::string target_frame = std::string("base_link");
std::string source_topic = std::string("scan");
std::string output_topic = std::string("base_scan");
std::string source_frame = std::string("primesense1_depth_frame");

ros::Publisher scan_pub;

struct polar_point {
    float r;
    float theta;
};

void polar_to_tf_point(polar_point& p_point, 
    std::string frame_id, tf::Stamped<tf::Point>& st_point)
{
    float x,y,z;
    x = p_point.r*cos(p_point.theta);
    y = p_point.r*sin(p_point.theta);
    z = 0;
    const tf::Point point = tf::Point(x,y,z);
    st_point = tf::Stamped<tf::Point>(point, ros::Time::now(), frame_id);
}

void st_point_to_polar_point(tf::Stamped<tf::Point>& st_point, polar_point& point)
{
    float x = st_point.getX();
    float y = st_point.getY();
    float r = pow((pow(x,2)+pow(y,2)),0.5);
    float theta = atan2(y,x);
    point.r = r;
    point.theta = theta;
}

void callback(const sensor_msgs::LaserScan& original_msg)
{
    if(source_frame.compare(original_msg.header.frame_id) != 0)
    {
        return;
    } 
    float o_t_min, o_t_max, o_t_inc;
    o_t_min = original_msg.angle_min;
    o_t_max = original_msg.angle_max;
    o_t_inc = original_msg.angle_increment;
    int num_points = (int)2.0*o_t_max/o_t_inc;
    sensor_msgs::LaserScan new_msg;
    tf::TransformListener transformer;
    for(int i=0; i<num_points; i++)
    {
        float theta = o_t_min+i*o_t_inc;
        float r = original_msg.ranges[i];
        polar_point point;
        point.r = r;
        point.theta = theta;
        tf::Stamped<tf::Point> old_point;
        polar_to_tf_point(point, original_msg.header.frame_id, old_point);
        tf::Stamped<tf::Point> st_point;
        geometry_msgs::PointStamped old_g_point;
        geometry_msgs::PointStamped st_g_point;
        tf::pointStampedTFToMsg(old_point, old_g_point);
        tf::pointStampedTFToMsg(st_point, st_g_point);
        try{
            transformer.transformPoint(target_frame,
                 old_g_point, st_g_point);
        }
        catch(tf::TransformException ex)
        {
            continue;
        }
        tf::pointStampedMsgToTF(st_g_point, st_point);
        st_point_to_polar_point(st_point, point);
        new_msg.ranges[i] = point.r;
        if(i == 0)
        {
            new_msg.angle_min = point.theta;
        }
        else if(i == num_points - 1)
        {
            new_msg.angle_max = point.theta;
        }
    }

    new_msg.header = original_msg.header;
    new_msg.header.frame_id = target_frame;
    new_msg.angle_increment = original_msg.angle_increment;
    new_msg.time_increment = original_msg.time_increment;
    new_msg.scan_time = original_msg.scan_time;
    new_msg.intensities = original_msg.intensities;
    scan_pub.publish(new_msg);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "parbot_laserscan_tranform");
    ros::NodeHandle n;

    /*if(ros::param::get("target_frame", target_frame) && 
        ros::param::get("source_frame", source_frame) && 
            ros::param::get("source_topic", source_topic) &&
                ros::param::get("output_topic", output_topic))
    {
    }   
    else
    {
        ROS_INFO("laserscan_transform error!!!!");
        ROS_INFO(target_frame.c_str());
        ROS_INFO(source_frame.c_str());
        ROS_INFO(source_topic.c_str());
        ROS_INFO(output_topic.c_str());
        return 1;
    }*/ 

    ros::Subscriber sub = n.subscribe(source_topic, 1, callback);
    scan_pub = n.advertise<sensor_msgs::LaserScan>(output_topic, 1);

    ros::spin();

    return 0;
}
The

The code now compiles and runs with the added try/catch but it never finds the transform. I am publishing a transform from base_link to primesense1_depth_frame via a TF to the primesense1_link which has a tf primesense1_depth_frame. I know that the TF's work because I can view them in rvis along with the laserscan messages from that sensor. Can anyone give me advice as to how to fix this?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-12-19 06:56:50 -0600

tfoote gravatar image

You should always use a try catch statement when calling tf methods. If you catch the error and let it retry on the next message you will likely see it start working. You have not given tf a chance to accumulate messages in the listener's buffer. By retrying on subsequent messages you will give the buffer time to fill.

There are several tf tutorials. This one is most relevant to your use case: http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C%2B%2B%29

edit flag offensive delete link more

Comments

I tried a try catch but still doesn't work info in question

AlphaOne gravatar imageAlphaOne ( 2013-12-19 07:03:21 -0600 )edit

The exception has a message which tells you what's wrong. You should try printing that out. And please actually do the tutorials. They are designed to teach you how to use the library and include hints on debugging things like this.

tfoote gravatar imagetfoote ( 2013-12-19 07:07:47 -0600 )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

2 followers

Stats

Asked: 2013-12-19 04:18:50 -0600

Seen: 3,261 times

Last updated: Dec 19 '13