0
$\begingroup$

Rosanswers logo

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?


Originally posted by AlphaOne on ROS Answers with karma: 141 on 2013-12-19

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by tfoote with karma: 58457 on 2013-12-19

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by AlphaOne on 2013-12-19:
I tried a try catch but still doesn't work info in question

Comment by tfoote on 2013-12-19:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.