How to transform a laserscan message with TF?
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?