helpers.h
Go to the documentation of this file.
00001 #ifndef _HELPERS_H_
00002 #define _HELPERS_H_
00003 
00004 #include <ros/ros.h>
00005 #include <tf/tf.h>
00006 
00007 #include <std_msgs/ColorRGBA.h>
00008 
00009 #include <geometry_msgs/Vector3.h>
00010 #include <geometry_msgs/Point.h>
00011 #include <geometry_msgs/Quaternion.h>
00012 #include <geometry_msgs/Pose.h>
00013 #include <geometry_msgs/Transform.h>
00014 
00015 #include "pcl/io/pcd_io.h"
00016 #include "pcl/point_types.h"
00017 #include "pcl/filters/passthrough.h"
00018 #include <pcl/features/normal_3d.h>
00019 #include <pcl_ros/transforms.h>
00020 
00021 #include <object_manipulator/tools/shape_tools.h>
00022 
00023 
00024 inline void setMatrixColumns(tf::Matrix3x3 &mat, tf::Vector3 &X, tf::Vector3 &Y, tf::Vector3 &Z)
00025 {
00026   mat.setValue( X.getX(), Y.getX(), Z.getX(),
00027                 X.getY(), Y.getY(), Z.getY(),
00028                 X.getZ(), Y.getZ(), Z.getZ() );
00029 }
00030 
00034 inline geometry_msgs::PoseStamped generateAlignedPose(const geometry_msgs::PoseStamped original_pose,
00035                                                       const geometry_msgs::Vector3 &axis,
00036                                                       const geometry_msgs::Vector3 &up)
00037 {
00038   tf::Pose pose_tf;
00039   tf::poseMsgToTF(original_pose.pose, pose_tf);
00040 
00041   tf::Point point = pose_tf.getOrigin();
00042 
00043   tf::Vector3 axis_tf;
00044   tf::vector3MsgToTF(axis, axis_tf);
00045 
00046   tf::Vector3 up_tf;
00047   tf::vector3MsgToTF(up, up_tf);
00048 
00049   tf::Matrix3x3 mat;
00050   tf::Vector3 X = axis_tf.normalized();
00051   tf::Vector3 Y = up_tf.cross(axis_tf).normalized();
00052   tf::Vector3 Z = X.cross(Y).normalized();
00053   setMatrixColumns(mat, X, Y, Z);
00054 
00055   point -= 0.15*X;
00056 
00057   pose_tf = tf::Pose(mat, point);
00058   geometry_msgs::PoseStamped output_pose;
00059   output_pose.header.frame_id = original_pose.header.frame_id;
00060 
00061   tf::poseTFToMsg(pose_tf, output_pose.pose);
00062   return output_pose;
00063 }
00064 
00068 inline geometry_msgs::PoseStamped generateRelativePose(geometry_msgs::PoseStamped original_pose, double roll, double pitch,
00069                      double yaw, double xdist, double ydist, double zdist)
00070 {
00071   tf::Pose pose_tf;
00072   tf::poseMsgToTF(original_pose.pose, pose_tf);
00073   tf::Transform T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw),
00074                                     tf::Vector3( xdist, ydist, zdist) );
00075   tf::Transform output_rot = pose_tf * T_rot;
00076   geometry_msgs::PoseStamped output_pose;
00077   output_pose.header.frame_id = original_pose.header.frame_id;
00078   tf::poseTFToMsg(output_rot, output_pose.pose);
00079   return output_pose;
00080 }
00081 
00082 
00083 template <class PointT>
00084     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)
00085 {
00086     pcl::PassThrough<PointT> pass;
00087 
00088     cloud_out = cloud_in;
00089     tf::Vector3 center(0,0,0);
00090 
00091     if(transform_cloud)
00092     {
00093 
00094         pcl_ros::transformPointCloudWithNormals( cloud_out, cloud_out, box.frame.inverse());
00095     }
00096     else {
00097         center = box.frame.getOrigin();
00098     }
00099 
00100     if(true)
00101     {
00102       pass.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cloud_out));
00103       pass.setFilterFieldName("z");
00104       pass.setFilterLimits ( center.getZ()-box.dims.getZ()/2, center.getZ()+box.dims.getZ()/2);
00105       pass.setFilterLimitsNegative (false);
00106       pass.filter(cloud_out);
00107     }
00108     if(true)
00109     {
00110       pass.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cloud_out));
00111       pass.setFilterFieldName("x");
00112       pass.setFilterLimits ( center.getX()-box.dims.getX()/2, center.getX()+box.dims.getX()/2);
00113       pass.setFilterLimitsNegative (false);
00114       pass.filter(cloud_out);
00115     }
00116     if(true)
00117     {
00118       pass.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cloud_out));
00119       pass.setFilterFieldName("y");
00120       pass.setFilterLimits ( center.getY()-box.dims.getY()/2, center.getY() + box.dims.getY()/2);
00121       pass.setFilterLimitsNegative (false);
00122       pass.filter(cloud_out);
00123     }
00124 
00125     if(transform_cloud && restore_frame) pcl_ros::transformPointCloudWithNormals( cloud_out, cloud_out, box.frame);
00126 
00127 }
00128 
00129 template <class PointT>
00130 void floating_box_filter(const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, const object_manipulator::shapes::Box &box)
00131 {
00132     // not yet implemented
00133 
00134 }
00135 
00136 
00137 //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));
00138 
00139 
00140 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Wed Jan 23 2013 15:27:54