transformation_publisher.cpp

Go to the documentation of this file.

#include <Transformation/transformation_publisher.h>
#ifndef Q_MOC_RUN
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>
#endif

Transformation_Publisher::Transformation_Publisher(boost::shared_ptr<Calibration_Object> object)
{
    joint_pub = nh.advertise<sensor_msgs::JointState>("calibration_transforms", 1);
    geometry_msgs::TransformStamped topToBase;
    geometry_msgs::TransformStamped topToMarkerLeft;
    geometry_msgs::TransformStamped topToMarkerRight;

    topToBase.header.frame_id = "object_top";
    topToBase.child_frame_id = "object_base";
    topToMarkerLeft.header.frame_id = "object_top";
    topToMarkerLeft.child_frame_id = "marker_left";
    topToMarkerRight.header.frame_id = "object_top";
    topToMarkerRight.child_frame_id = "marker_right";
    mapToTop.header.frame_id = "calibration_center";
    mapToTop.child_frame_id = "object_top";
    topToScanFrame.header.frame_id = "object_top";
    topToScanFrame.child_frame_id = "object_scan_frame";
    markerLeftToCamera.header.frame_id = "marker_left";
    markerLeftToCamera.child_frame_id = "camera_frame";
    markerRightToCamera.header.frame_id = "marker_right";
    markerRightToCamera.child_frame_id = "camera_frame";

    topToBase.transform.translation.z = -object->side_b;
    topToBase.transform.rotation =  tf::createQuaternionMsgFromYaw(0.0);
    Eigen::Affine3d eigenTransform = Eigen::Affine3d(object->frame_marker_left);
    tf::Transform tfTransform;
    geometry_msgs::Transform gmTransform;
    tf::transformEigenToTF(eigenTransform, tfTransform);
    tf::transformTFToMsg(tfTransform, gmTransform);
    topToMarkerLeft.transform = gmTransform;
    eigenTransform = Eigen::Affine3d(object->frame_marker_right);
    tf::transformEigenToTF(eigenTransform, tfTransform);
    tf::transformTFToMsg(tfTransform, gmTransform);
    topToMarkerRight.transform = gmTransform;
    mapToTop.transform.rotation =  tf::createQuaternionMsgFromYaw(0.0);
    topToScanFrame.transform.rotation =  tf::createQuaternionMsgFromYaw(0.0);

    staticTransforms.push_back(topToBase);
    staticTransforms.push_back(topToMarkerLeft);
    staticTransforms.push_back(topToMarkerRight);
}

void Transformation_Publisher::newTransform_Laserscan(const Eigen::Matrix4d& transform, double distanceToTop)
{
    Eigen::Affine3d eigenTransform = Eigen::Affine3d(transform);
    tf::Transform tfTransform;
    geometry_msgs::Transform gmTransform;
    tf::transformEigenToTF(eigenTransform, tfTransform);
    tf::transformTFToMsg(tfTransform, gmTransform);
    mapToTop.transform = gmTransform;

    topToScanFrame.transform.translation.z = -distanceToTop;
    topToScanFrame.transform.rotation =  tf::createQuaternionMsgFromYaw(0.0);
}

void Transformation_Publisher::newTransform_Camera(const Eigen::Matrix4d& transform, Markertype markertype)
{
    Eigen::Affine3d eigenTransform = Eigen::Affine3d(transform);
    tf::Transform tfTransform;
    geometry_msgs::Transform gmTransform;
    tf::transformEigenToTF(eigenTransform, tfTransform);
    tf::transformTFToMsg(tfTransform, gmTransform);

    if (markertype == Markertype::LEFT)
    {
        ROS_INFO("Got new camera frame from left marker.");
        markerLeftToCamera.transform = gmTransform;
    }
    else if (markertype == Markertype::RIGHT)
    {
        ROS_INFO("Got new camera frame from right marker.");
        markerRightToCamera.transform = gmTransform;
    }
    currentMarkertype = markertype;
}

void Transformation_Publisher::run()
{
    ros::Rate loop_rate(30);
    while (ros::ok())
    {
        //Publish static transforms
        for (unsigned int i = 0; i < staticTransforms.size(); i++)
        {
            staticTransforms[i].header.stamp = ros::Time::now();
            broadcaster.sendTransform(staticTransforms[i]);
        }

        mapToTop.header.stamp = ros::Time::now();
        broadcaster.sendTransform(mapToTop);

        topToScanFrame.header.stamp = ros::Time::now();
        broadcaster.sendTransform(topToScanFrame);

        try
        {
            if (currentMarkertype == Markertype::LEFT)
            {
                markerLeftToCamera.header.stamp = ros::Time::now();
                broadcaster.sendTransform(markerLeftToCamera);
            }
            else if (currentMarkertype == Markertype::RIGHT)
            {
                markerRightToCamera.header.stamp = ros::Time::now();
                broadcaster.sendTransform(markerRightToCamera);
            }
        }
        catch (...)
        {
            ROS_ERROR("Something went wrong in the tf broadcaster");
        }

        loop_rate.sleep();
    }
}

geometry_msgs::Transform Transformation_Publisher::matrixToTransform(const Eigen::Matrix4d& matrix)
{
    Eigen::Affine3d eigenTransform = Eigen::Affine3d(matrix);
    tf::Transform tfTransform;
    geometry_msgs::Transform gmTransform;
    tf::transformEigenToTF(eigenTransform, tfTransform);
    tf::transformTFToMsg(tfTransform, gmTransform);
    return gmTransform;
}