transform_shape_array.cpp

Go to the documentation of this file.

// ROS core
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <cob_3d_mapping_msgs/ShapeArray.h>
#include <cob_3d_mapping_common/ros_msg_conversions.h>


using namespace std;
using namespace cob_3d_mapping;

class TransformShapeArray
{
//protected:
//  ros::NodeHandle nh_;

public:
  string target_frame_;

  tf::TransformListener tf_listener_;
  ros::Subscriber sub_;
  ros::Publisher pub_;

  TransformShapeArray() :
    target_frame_("/map")
  {
    ros::NodeHandle private_nh("~");
    pub_ = private_nh.advertise<cob_3d_mapping_msgs::ShapeArray>("output",1);
    sub_ = private_nh.subscribe ("input", 1,  &TransformShapeArray::cloud_cb, this);

    private_nh.getParam ("target_frame", target_frame_);
  }

  // Callback
  void
  cloud_cb (const cob_3d_mapping_msgs::ShapeArray::ConstPtr sa_in)
  {
    if(sa_in->header.frame_id != target_frame_)
    {
      Eigen::Affine3f af_target = Eigen::Affine3f::Identity();
      tf::StampedTransform trf_map;

      try
      {
        tf_listener_.waitForTransform(target_frame_, sa_in->header.frame_id, sa_in->header.stamp, ros::Duration(2));
        tf_listener_.lookupTransform(target_frame_, sa_in->header.frame_id, sa_in->header.stamp, trf_map);
      }
      catch (tf::TransformException& ex) { ROS_ERROR("[transform shape array] : %s",ex.what()); return; }

      Eigen::Affine3d ad;
      tf::transformTFToEigen(trf_map, ad);
      af_target = ad.cast<float>();
      cob_3d_mapping_msgs::ShapeArray sa_out;
      sa_out.header = sa_in->header;
      sa_out.header.frame_id = target_frame_;
      for (unsigned int i = 0; i < sa_in->shapes.size (); i++)
      {
        cob_3d_mapping_msgs::Shape s;
        s.header = sa_in->header;
        s.header.frame_id = target_frame_;
        if( sa_in->shapes[i].type == cob_3d_mapping_msgs::Shape::POLYGON)
        {
          Polygon::Ptr poly_ptr (new Polygon());
          fromROSMsg(sa_in->shapes[i], *poly_ptr);
          poly_ptr->transform(af_target);
          toROSMsg(*poly_ptr,s);
        }
        else if( sa_in->shapes[i].type == cob_3d_mapping_msgs::Shape::CYLINDER)
        {
          Cylinder::Ptr cyl_ptr (new Cylinder());
          fromROSMsg(sa_in->shapes[i], *cyl_ptr);
          cyl_ptr->transform(af_target);
          toROSMsg(*cyl_ptr,s);
        }
        sa_out.shapes.push_back (s);
      }
      pub_.publish(sa_out);
    }
    else
      pub_.publish(*sa_in);
  }

};


int
main (int argc, char** argv)
{
  ros::init (argc, argv, "transform_shape_array");

  TransformShapeArray b;
  ros::spin ();

  return (0);
}