helpers.h

Go to the documentation of this file.

#ifndef _HELPERS_H_
#define _HELPERS_H_

#include <ros/ros.h>
#include <tf/tf.h>

#include <std_msgs/ColorRGBA.h>

#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Transform.h>

#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include "pcl/filters/passthrough.h"
#include <pcl/features/normal_3d.h>
#include <pcl_ros/transforms.h>

#include <object_manipulator/tools/shape_tools.h>


inline void setMatrixColumns(tf::Matrix3x3 &mat, tf::Vector3 &X, tf::Vector3 &Y, tf::Vector3 &Z)
{
  mat.setValue( X.getX(), Y.getX(), Z.getX(),
                X.getY(), Y.getY(), Z.getY(),
                X.getZ(), Y.getZ(), Z.getZ() );
}

inline geometry_msgs::PoseStamped generateAlignedPose(const geometry_msgs::PoseStamped original_pose,
                                                      const geometry_msgs::Vector3 &axis,
                                                      const geometry_msgs::Vector3 &up)
{
  tf::Pose pose_tf;
  tf::poseMsgToTF(original_pose.pose, pose_tf);

  tf::Point point = pose_tf.getOrigin();

  tf::Vector3 axis_tf;
  tf::vector3MsgToTF(axis, axis_tf);

  tf::Vector3 up_tf;
  tf::vector3MsgToTF(up, up_tf);

  tf::Matrix3x3 mat;
  tf::Vector3 X = axis_tf.normalized();
  tf::Vector3 Y = up_tf.cross(axis_tf).normalized();
  tf::Vector3 Z = X.cross(Y).normalized();
  setMatrixColumns(mat, X, Y, Z);

  point -= 0.15*X;

  pose_tf = tf::Pose(mat, point);
  geometry_msgs::PoseStamped output_pose;
  output_pose.header.frame_id = original_pose.header.frame_id;

  tf::poseTFToMsg(pose_tf, output_pose.pose);
  return output_pose;
}

inline geometry_msgs::PoseStamped generateRelativePose(geometry_msgs::PoseStamped original_pose, double roll, double pitch,
                     double yaw, double xdist, double ydist, double zdist)
{
  tf::Pose pose_tf;
  tf::poseMsgToTF(original_pose.pose, pose_tf);
  tf::Transform T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
                                    tf::Vector3( xdist, ydist, zdist) );
  tf::Transform output_rot = pose_tf * T_rot;
  geometry_msgs::PoseStamped output_pose;
  output_pose.header.frame_id = original_pose.header.frame_id;
  tf::poseTFToMsg(output_rot, output_pose.pose);
  return output_pose;
}


template <class PointT>
    void box_filter(const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, const object_manipulator::shapes::Box &box, bool transform_cloud = false, bool restore_frame = true)
{
    pcl::PassThrough<PointT> pass;

    cloud_out = cloud_in;
    tf::Vector3 center(0,0,0);

    if(transform_cloud)
    {

        pcl_ros::transformPointCloudWithNormals( cloud_out, cloud_out, box.frame.inverse());
    }
    else {
        center = box.frame.getOrigin();
    }

    if(true)
    {
      pass.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cloud_out));
      pass.setFilterFieldName("z");
      pass.setFilterLimits ( center.getZ()-box.dims.getZ()/2, center.getZ()+box.dims.getZ()/2);
      pass.setFilterLimitsNegative (false);
      pass.filter(cloud_out);
    }
    if(true)
    {
      pass.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cloud_out));
      pass.setFilterFieldName("x");
      pass.setFilterLimits ( center.getX()-box.dims.getX()/2, center.getX()+box.dims.getX()/2);
      pass.setFilterLimitsNegative (false);
      pass.filter(cloud_out);
    }
    if(true)
    {
      pass.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cloud_out));
      pass.setFilterFieldName("y");
      pass.setFilterLimits ( center.getY()-box.dims.getY()/2, center.getY() + box.dims.getY()/2);
      pass.setFilterLimitsNegative (false);
      pass.filter(cloud_out);
    }

    if(transform_cloud && restore_frame) pcl_ros::transformPointCloudWithNormals( cloud_out, cloud_out, box.frame);

}

template <class PointT>
void floating_box_filter(const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, const object_manipulator::shapes::Box &box)
{
    // not yet implemented

}


//void drawBox(ros::Publisher &pub, Box box, std::string ns = std::string("debug"), int id = 0, const ros::Duration lifetime = ros::Duration(), const std_msgs::ColorRGBA color = msg::createColorMsg(0.5, 0.5, 0.5, 0.5));


#endif