handle.cpp

Go to the documentation of this file.

//This programe gets the pose of a marker above a door handle
//and uses it to extract the points that belong to the handle geometrically
//The assumptions about the position of the door handle are stated in
//the setFilterLimits calls
#include "ros/ros.h"
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h> 
#include <geometry_msgs/Pose.h> 
#include <tf/transform_datatypes.h>
#include <pcl_ros/transforms.h>
#include <pcl/ros/conversions.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include "tools.h"

// head control
//#include <pr2_controllers_msgs/PointHeadAction.h>

#include <iostream>

#include <tf/transform_listener.h>

//tf::Transform checkerboardPose;
tf::TransformListener* tflistener;
ros::Publisher handle_pub, pc_pub, vis_pub, pregrasp_pub, postgrasp_pub, grasp_pub;
std::string global_checkerboard_frame;
std::string global_torso_frame;


void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
  // tf stuff
  tf::StampedTransform door_to_torso;
  if(tflistener->waitForTransform(global_torso_frame, msg->header.frame_id, msg->header.stamp, ros::Duration(0.5))){
    tflistener->lookupTransform(global_torso_frame,  msg->header.frame_id, msg->header.stamp, door_to_torso);
  } else {
    ROS_WARN("No tranform from %s to %s available", msg->header.frame_id.c_str(), global_torso_frame.c_str());
    return;
  }
  tf::Stamped<tf::Pose> handle_pose_in_door_frame; 
  tf::poseStampedMsgToTF(*msg, handle_pose_in_door_frame);
  tf::Transform handle_pose_in_torso_frame = door_to_torso * handle_pose_in_door_frame; 
  tf::Transform grasp = handle_pose_in_torso_frame * tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0.05,0,0)); 

  //Grasp: Same as msg
  tf::Stamped<tf::Pose> graspTF(grasp, msg->header.stamp,global_torso_frame); 
  geometry_msgs::PoseStamped graspMsg;
  tf::poseStampedTFToMsg(graspTF, graspMsg);

  //Pregrasp: 10cm in front of grasp

  tf::Transform pregrasp(handle_pose_in_torso_frame);  
  pregrasp.getOrigin().setX(pregrasp.getOrigin().getX() - 0.08);
  tf::Stamped<tf::Pose> pregraspTF(pregrasp, msg->header.stamp,global_torso_frame); 
  geometry_msgs::PoseStamped pregraspMsg;
  tf::poseStampedTFToMsg(pregraspTF, pregraspMsg);
  
  //Postgrasp rotate Z downwards by +-45° around X (depending on which side it is)
  tf::Vector3 handle_tip_in_handle_frame(0,0,1); // Z Axis always points to door hinge along handle
  tf::Vector3 handle_tip_in_torso_frame = door_to_torso * handle_pose_in_door_frame * handle_tip_in_handle_frame - handle_pose_in_torso_frame.getOrigin();

  tf::Vector3 rotation_axis = handle_tip_in_torso_frame.cross(tf::Vector3(0,0,1));
  rotation_axis.normalize();
  ROS_DEBUG("rotation_axis (in torso frame) xyz= %f %f %f", rotation_axis.getX(), rotation_axis.getY(), rotation_axis.getY());
  
  tf::Quaternion grasp_to_postgrasp_rot(rotation_axis, -60.0/180.0*3.1415927);
  tf::Transform grasp_to_postgrasp(grasp_to_postgrasp_rot, tf::Vector3(0.05,0,0));//open door slightly so it doesn't fall back to latched
  tf::Transform postgrasp = grasp * grasp_to_postgrasp;

  //Grasp: Same as msg
  tf::Stamped<tf::Pose> postgraspTF(postgrasp, msg->header.stamp,global_torso_frame); 
  geometry_msgs::PoseStamped postgraspMsg;
  tf::poseStampedTFToMsg(postgraspTF, postgraspMsg);

  if (pregrasp_pub.getNumSubscribers() > 0){
    pregrasp_pub.publish(pregraspMsg);
  }
  if (grasp_pub.getNumSubscribers() > 0){
    grasp_pub.publish(graspMsg);
  }
  if (postgrasp_pub.getNumSubscribers() > 0){
    postgrasp_pub.publish(postgraspMsg);
  }

}

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  ROS_DEBUG_THROTTLE(5, "Received a point cloud: [%d, %d, %d] (This message is only printed every 5sec)", msg->width, msg->height, (int)msg->data.size());

  // tf stuff
  tf::StampedTransform torso_transform;
  tf::StampedTransform optical_transform;
  if(tflistener->waitForTransform(global_torso_frame, global_checkerboard_frame,msg->header.stamp, ros::Duration(0.5))){
    tflistener->lookupTransform(global_torso_frame, global_checkerboard_frame, msg->header.stamp, torso_transform);
    ROS_DEBUG("Checkerboard xyz= %f %f %f", torso_transform.getOrigin().getX(), torso_transform.getOrigin().getY(), torso_transform.getOrigin().getY());
  } else {
    ROS_INFO_THROTTLE(30, "No tranform from %s to %s available, probably no checkerboard in sight (or cb-detector not started).", global_torso_frame.c_str(), global_checkerboard_frame.c_str());
    return;
  }
  if(tflistener->waitForTransform(global_checkerboard_frame, msg->header.frame_id, msg->header.stamp, ros::Duration(5.0/30.0))){
    tflistener->lookupTransform(global_checkerboard_frame, msg->header.frame_id,  msg->header.stamp, optical_transform);
  } else {
    ROS_WARN("No tranform from %s to %s available", global_checkerboard_frame.c_str(), msg->header.frame_id.c_str());
    return;
  }
  // end tf stuff

  //Filtering the cloud in checkerboard Frame ##################################
  myCloudType::Ptr pc (new myCloudType );
  //TODO check whether checkerboardPose is identity and return if so
  pcl::fromROSMsg(*msg, *pc);
  pcl_ros::transformPointCloud(*pc, *pc, optical_transform);//pc is now in checkerboard frame

  //This filter settings should remove most points in and behind the door plane
  float xmin = -0.10,xmax = 0.25,ymin = 0.00,ymax = 0.32,zmin = -.2 ,zmax = -.03;
  if (vis_pub.getNumSubscribers() > 0)
    vis_pub.publish(visualize_filter_box(xmin,xmax,ymin,ymax,zmin,zmax, pc->header));
  box_filter(pc, xmin,xmax,ymin,ymax,zmin,zmax);

  if(pc->points.size() == 0){
    ROS_INFO_THROTTLE(5, "Empty Result Set");
    ros::Duration(0.5).sleep(); // sleep for half a second
    return;
  }

  pcl_ros::transformPointCloud(*pc, *pc, torso_transform);//pc is now in torso_lift_link
  pc->header.frame_id =global_torso_frame;
  sensor_msgs::PointCloud2 outmsg;
  pcl::toROSMsg(*pc,outmsg);
  if (pc_pub.getNumSubscribers() > 0)
    pc_pub.publish(outmsg);

  //Computation of the Desired Gripper Transformations #########################
  Eigen::Vector3f mean, principalAxis;
  if(!computeMainAxis(*pc, mean, principalAxis, 20.0)) return; //main axis eval is 100 times next eval
  //std::cout << "\nReturned principalAxis:\n" << principalAxis << "\nReturned Mean:\n" << mean << std::endl;
  if (!((principalAxis.array() == principalAxis.array())).all() || //checking for nan
      !((mean.array() == mean.array())).all()) ROS_WARN("No valid axis for grasp");
  ROS_DEBUG("Mean xyz= %f %f %f", mean(0), mean(1), mean(2));

  Eigen::Matrix4f torso_trans_mat;
  pcl_ros::transformAsMatrix(torso_transform, torso_trans_mat);
  Eigen::Matrix3f torso_rot_mat = torso_trans_mat.block<3,3>(0,0);
  //std::cout << std::endl << torso_rot_mat << std::endl;

  //DOOR NoRMAL GOES TOWARDS ROBOT
  Eigen::Vector3f door_normal(0,0,mean(2) - torso_transform.getOrigin().getZ()); //in checkerboard coord frame
  door_normal.normalize();
  door_normal = torso_rot_mat * door_normal; //door normal in torso coordinate frame
  Eigen::Vector3f handle_axis(principalAxis); //in torso_lift_link. Shall be gripper z axis
  Eigen::Vector3f y_axis  = handle_axis.cross(door_normal); //don't care about this, depends on the other two
  y_axis.normalize();
  door_normal  = y_axis.cross(handle_axis); //adapt door normal to be orthogonal to handle axis 
  ROS_DEBUG_STREAM("Door Normal:\n" << door_normal);
  ROS_DEBUG_STREAM("Handle Axis:\n" << handle_axis);
  ROS_DEBUG_STREAM("Third Axis:\n"  << y_axis);
  Eigen::Matrix3f grm;//gripper_rot_mat;
  grm << -door_normal, -y_axis, handle_axis;
  ROS_DEBUG_STREAM("Desired Gripper Coord Frame\n: " << grm);

  //gripper z should be aligned with the handle, gripper x with the door normal

  //This is the transpose (and since orthonormal, the inverse) of the desired gripper coord frame
  tf::Matrix3x3 toGripperTargetCoordFrame(grm(0,0), grm(0,1), grm(0,2), 
                                        grm(1,0), grm(1,1), grm(1,2), 
                                        grm(2,0), grm(2,1), grm(2,2)) ;

  tf::Quaternion gripperRotation; 
  toGripperTargetCoordFrame.getRotation(gripperRotation);

  //TODO: third axis and transform-rotation from that axis
  geometry_msgs::PoseStamped handle_pose = getPoseStamped(mean, gripperRotation); 
  if (handle_pub.getNumSubscribers() > 0)
    handle_pub.publish(handle_pose);

  //tf::Vector3 gripperPosition(mean(0), mean(1), mean(2));
  //tf::Transform grasp_tf(gripperRotation, gripperRotation);
  //tf::Stamped<tf::Pose> grasp_tf_pose(grasp_tf);
  //geometry_msgs::PoseStamped msg;
  //tf::poseStampedTFToMsg      (grasp_tf_pose, msg);

//geometry_msgs::Pose makePose(tf::Transform t){
  geometry_msgs::PoseStamped pregrasp, grasp, postgrasp;
  //this should be 10 cm along the door normal towards the robot
  pregrasp = getPoseStamped(mean+0.16*door_normal, gripperRotation);
  pregrasp.header.stamp = msg->header.stamp;
  if (pregrasp_pub.getNumSubscribers() > 0)
    pregrasp_pub.publish(pregrasp);

  grasp = getPoseStamped(mean-(0.03*door_normal), gripperRotation);
  grasp.header.stamp = msg->header.stamp;
  if (grasp_pub.getNumSubscribers() > 0)
    grasp_pub.publish(grasp);

  ROS_DEBUG_COND(mean(1) > torso_transform.getOrigin().getY(), "Mean left of checkerboard");
  double rot_dir = -1.0;//mean(1) > torso_transform.getOrigin().getY() ? -1.0 : 1.0 ; //switch rotation depending on the side of the handle axis
  tf::Quaternion handleRotation(tf::Vector3(door_normal(0),door_normal(1),door_normal(2)), rot_dir/4.0*M_PI );
  gripperRotation *= handleRotation;
  postgrasp.header.stamp = msg->header.stamp;
  postgrasp.header.frame_id = global_torso_frame;
  postgrasp.pose.position.x = mean(0)- 0.05 * door_normal(0) + 0.05;
  postgrasp.pose.position.y = mean(1)- 0.05 * door_normal(1) + rot_dir * 0.02 * principalAxis(1);
  postgrasp.pose.position.z = mean(2)- 0.05 * door_normal(2) - 0.08 ; //torso_lift_link z is always up
  postgrasp.pose.orientation.w = gripperRotation.getW();
  postgrasp.pose.orientation.x = gripperRotation.getX();
  postgrasp.pose.orientation.y = gripperRotation.getY();
  postgrasp.pose.orientation.z = gripperRotation.getZ();
  if (postgrasp_pub.getNumSubscribers() > 0)
    postgrasp_pub.publish(postgrasp);
  ROS_INFO_THROTTLE(5, "Published all grasping poses");
  //grasp_handle(pregrasp.pose, grasp.pose, postgrasp.pose); 
}

/*
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
  tf::Stamped<tf::Pose> bt;
  poseStampedMsgToTF(*msg, bt);
  checkerboardPose = bt;
  //make sure y axis goes upwards
  if(checkerboardPose.getOrigin().getZ() < 0.0){
    checkerboardPose *= tf::Transform(tf::Quaternion(tf::Vector3(0,1,0), M_PI));
  }
}
*/
int main(int argc, char **argv)
{
  ros::init(argc, argv, "handle_grasp_computation");
  ros::NodeHandle n;
  tflistener = new tf::TransformListener; 
  global_checkerboard_frame = "/checkerboard_0";
  global_torso_frame = "/torso_lift_link";

  pc_pub = n.advertise<sensor_msgs::PointCloud2>("handle_segment", 1);
  vis_pub = n.advertise<visualization_msgs::Marker>( "/visualization_marker", 2 );
  pregrasp_pub = n.advertise<geometry_msgs::PoseStamped>( "pregrasp", 3 );
  grasp_pub = n.advertise<geometry_msgs::PoseStamped>( "grasp", 3 );
  postgrasp_pub = n.advertise<geometry_msgs::PoseStamped>( "postgrasp", 3 );
  handle_pub = n.advertise<geometry_msgs::PoseStamped>( "handle", 3 );

//  ros::Subscriber subPS = n.subscribe("/pose0", 10, poseCallback); //this comes from the checkerboard_detector2
  //Subscribe to the point cloud containing the handle and extract 
  ros::Subscriber subPC = n.subscribe("cloud_in", 10, pointCloudCallback);
  ros::Subscriber subPose = n.subscribe("door_handle", 10, poseCallback);
  ROS_INFO("Handle extractor started");
  //Multithreaded spinner because waitfortransform in one callback may blocks the other callback otherwise
  ros::MultiThreadedSpinner spinner(2); // Use 2 threads
  spinner.spin(); // spin() will not return until the node has been shutdown

}