create_object_model_server.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

// Author(s): Kaijen Hsiao

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <tf/transform_listener.h>
#include <pcl/io/io.h>
#include "tabletop_object_detector/SegmentObjectInHand.h"
#include "pr2_create_object_model/ModelObjectInHandAction.h"
#include "pr2_create_object_model/ObjectInHand.h"
#include "object_manipulator/tools/mechanism_interface.h"
#include "motion_planning_msgs/LinkPadding.h"
#include "motion_planning_msgs/OrderedCollisionOperations.h"
#include <eigen_conversions/eigen_msg.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <object_manipulation_msgs/ClusterBoundingBox.h>

#include <tabletop_collision_map_processing/collision_map_interface.h>

using pr2_create_object_model::ObjectInHand;

class InHandObjectModeler
{
 
private:

  ros::NodeHandle root_nh_;
  
  ros::NodeHandle priv_nh_;

  actionlib::SimpleActionServer<pr2_create_object_model::ModelObjectInHandAction> model_object_action_server_;

  pr2_create_object_model::ModelObjectInHandResult model_object_result_;

  ros::Publisher pub_cloud;

  std::string segment_object_service_name_;

  object_manipulator::MechanismInterface mech_interface_;

  tabletop_collision_map_processing::CollisionMapInterface collision_map_interface_;

  bool stereo_;

public:
  
  InHandObjectModeler() : 
    root_nh_(""),
    priv_nh_("~"),
    model_object_action_server_(priv_nh_, "/model_object_in_hand_action", 
                                boost::bind(&InHandObjectModeler::executeCB, this, _1), false)
  {
    segment_object_service_name_ = "/segment_object_in_hand_srv";

    while ( !ros::service::waitForService(segment_object_service_name_, ros::Duration().fromSec(3.0)) && priv_nh_.ok() )
    {
      ROS_INFO("Waiting for service %s...", segment_object_service_name_.c_str());
    }
    if (!priv_nh_.ok()) exit(0);

    pub_cloud = priv_nh_.advertise<ObjectInHand>("modeled_object_in_hand", 1);

    priv_nh_.param<bool>("stereo", stereo_, true);

    model_object_action_server_.start();
    ROS_INFO("started action server");
  }

  void executeCB(const pr2_create_object_model::ModelObjectInHandGoalConstPtr &goal)
  {
    sensor_msgs::PointCloud2 object_in_hand;
    sensor_msgs::PointCloud2 cluster;

    //Set the min_disparity high so we can see stuff closer to the camera
    if(stereo_)
    {
      int res = system("rosrun dynamic_reconfigure dynparam set_from_parameters /narrow_stereo_textured/narrow_stereo_textured_proc _min_disparity:=30");
    }    
    bool result;

    //not supposed to move the arm--just grab any points we see in the hand and return them
    if(!goal->move_arm){
      result = callSegmentObjectInHand(goal->arm_name, cluster);
      if(result != 0)
      {
        object_in_hand = cluster;
      }
    }
    else
    {
      //move the arm to where the camera can see it
      moveArmIntoView(goal->arm_name);

      //turn the hand towards the robot, gathering clusters as we go
      turnArmInwards(goal->arm_name, object_in_hand, goal->keep_level);
    }

    model_object_result_.cluster = object_in_hand;
    
    std::string collision_name;
    if (goal->add_to_collision_map && !object_in_hand.data.empty())
    {
      try
      {
        bool success = true;
        ros::Time start_time = ros::Time::now();
        while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
        {
          if (!priv_nh_.ok() || ros::Time::now() - start_time >= ros::Duration(5.0)) 
          {
            success = false;
            break;
          }
        }
        if (success)
        {
          //remove any current object in the hand from the collision map
          //*** add function for removing a single attached object to collision_map_interface.cpp
          if(goal->arm_name == "right_arm")
          {
          }
          else if(goal->arm_name == "left_arm")
          {
          }

          //convert the object cluster to PointCloud1
          sensor_msgs::PointCloud cluster1;
          sensor_msgs::convertPointCloud2ToPointCloud(object_in_hand, cluster1);
          /*
          collision_map_interface_.processCollisionGeometryForCluster(cluster1, collision_name);
          //give the cluster collision object time to instantiate
          ros::Duration(1.0).sleep();
          */

          //compute the cluster bounding box and add it to collision map
          object_manipulation_msgs::ClusterBoundingBox bbox;
          collision_map_interface_.getClusterBoundingBox(cluster1, bbox.pose_stamped, bbox.dimensions);
          collision_map_interface_.processCollisionGeometryForBoundingBox(bbox, collision_name);
          //give the cluster collision object time to instantiate
          ros::Duration(0.5).sleep();

          mech_interface_.attachObjectToGripper(goal->arm_name, collision_name);
        }
        if (!success) 
        {
          ROS_ERROR("Create object model: failed to add object to collision map");
          collision_name.clear();
        }
      }
      catch(...)
      {
        ROS_ERROR("Error while trying to add object to collision map");
        collision_name.clear();
      }
    }

    //publish the final result
    ObjectInHand oih;
    oih.cluster = object_in_hand;
    oih.arm_name = goal->arm_name;
    oih.collision_name = collision_name;
    model_object_result_.collision_name = collision_name;
    ROS_INFO("frame_id: %s, final num points: %d, collision name: %s", 
             object_in_hand.header.frame_id.c_str(), (int)object_in_hand.width, collision_name.c_str());
    pub_cloud.publish(oih);

    //Set the min_disparity back
    if(stereo_)
    {
      int res = system("rosrun dynamic_reconfigure dynparam set_from_parameters /narrow_stereo_textured/narrow_stereo_textured_proc _min_disparity:=0");
    }
    model_object_action_server_.setSucceeded(model_object_result_);
  }

  bool callSegmentObjectInHand(std::string arm_name, sensor_msgs::PointCloud2 &object_in_hand)
  {
    tabletop_object_detector::SegmentObjectInHand segmentation_srv;
    if (arm_name.compare("right_arm") == 0)
      segmentation_srv.request.wrist_frame = std::string("r_wrist_roll_link");
    else
      segmentation_srv.request.wrist_frame = std::string("l_wrist_roll_link");
    if (!ros::service::call(segment_object_service_name_, segmentation_srv))
    {
      ROS_ERROR("Call to segmentation service failed");
      return 0;
    }
    object_in_hand = segmentation_srv.response.cluster;
    return 1;
  }

  void pointHead(std::string arm_name)
  {
    geometry_msgs::PointStamped desired_head_point;
    if(stereo_)
    {
      if (arm_name.compare("right_arm") == 0)
        desired_head_point.header.frame_id = "r_wrist_roll_link";
      else
        desired_head_point.header.frame_id = "l_wrist_roll_link";
      desired_head_point.point.x = .2;
      mech_interface_.pointHeadAction(desired_head_point, "/narrow_stereo_optical_frame");
    }
    else
    {
      geometry_msgs::PoseStamped gripper_pose = mech_interface_.getGripperPose(arm_name, std::string("torso_lift_link"));
      desired_head_point.header.frame_id = "torso_lift_link";
      desired_head_point.point.x = gripper_pose.pose.position.x;
      desired_head_point.point.y = gripper_pose.pose.position.y;
      desired_head_point.point.z = gripper_pose.pose.position.z - .4;
      mech_interface_.pointHeadAction(desired_head_point, "/openni_rgb_optical_frame");
    }
    ROS_INFO("pointing head");
  }

  bool moveArmIntoView(std::string arm_name)
  {
    geometry_msgs::PoseStamped desired_pose = mech_interface_.getGripperPose(arm_name, std::string("torso_lift_link"));
    desired_pose.pose.position.x = .7;
    desired_pose.pose.position.y = 0;
    desired_pose.pose.position.z = 0.20;
    mech_interface_.moveArmToPoseCartesian(arm_name, desired_pose, ros::Duration(6.0), .015, .09, .03, .2, .1);
    ROS_INFO("moving arm in front of head");

    pointHead(arm_name);
    ros::Duration(0.5).sleep();

    return 1;
  }

  bool turnArmInwards(std::string arm_name, sensor_msgs::PointCloud2 &object_in_hand, int keep_level)
  {
    geometry_msgs::PoseStamped current_pose = mech_interface_.getGripperPose(arm_name, std::string("torso_lift_link"));
    Eigen::eigen2_Transform3d current_trans;
    tf::poseMsgToEigen(current_pose.pose, current_trans);
    Eigen::eigen2_Transform3d desired_trans;

    int num_steps = 0;
    double rot_angle;
    std::vector<geometry_msgs::PoseStamped> desired_poses;
    Eigen::Vector3d z(0,0,1);

    // //don't need to keep the object level--set the ultimate desired pose+posture to a predetermined good view
    // if(!keep_level)
    // {
    // }
    // //need to keep the object level--compute desired poses that try to turn the gripper inwards
    // else
    if(1)
    {
      //find the angle that rotates the current gripper x-axis to the torso_lift_link x-z plane, pointed in the -x direction
      //but make the gripper swing towards the center 
      //(for the right arm, -PI/4 moves +3PI/4, left arm reversed)
      Eigen::Vector3d x_axis = current_trans.rotation().col(0);
      rot_angle = atan2(x_axis[1], -x_axis[0]);
      std::cout << "rot_angle before:" << rot_angle << std::endl;
      if(arm_name.compare("right_arm") == 0 && rot_angle < -M_PI/4) rot_angle += 2*M_PI;
      if(arm_name.compare("left_arm") == 0 && rot_angle > M_PI/4) rot_angle -= 2*M_PI; 
      //std::cout << "x_axis:\n" << x_axis << std::endl;
      std::cout << "rot_angle after:" << rot_angle << std::endl;

      //compute the number of steps based on the max angle step to move before trying to snap a point cloud
      double max_angle_step = M_PI/8;
      num_steps = ceil(fabs(rot_angle) / max_angle_step) + 1;
      std::cout << "num_steps:" << num_steps << std::endl;

      //compute the sequence of desired poses at which to snap point clouds by interpolating
      Eigen::eigen2_Transform3d step_trans;     
      geometry_msgs::PoseStamped desired_pose;
      for(int view_ind=0; view_ind<num_steps; view_ind++)        
      {
        double frac;
        if(num_steps == 1) frac = 1;
        else frac = view_ind / float(num_steps-1);
        //std::cout << "frac:" << frac << std::endl;
        Eigen::eigen2_AngleAxis<double> step_rotation(rot_angle*frac, z);
        step_trans = step_rotation * current_trans;
        for(int i=0; i<3; i++) step_trans(i,3) = current_trans(i,3);
        tf::poseEigenToMsg(step_trans, desired_pose.pose);      
        desired_pose.header.frame_id = "torso_lift_link";
        desired_pose.header.stamp = ros::Time::now();
        desired_poses.push_back(desired_pose);
        //std::cout << "desired pose:" << desired_pose << std::endl;
      }
      Eigen::eigen2_Transform3d towards_trans = step_trans;
      //now go PI towards the center if we haven't gone that far already
      if(fabs(rot_angle) < M_PI)
      {
        double additional_angle = -M_PI;
        if(arm_name.compare("left_arm") == 0) additional_angle = M_PI;
        int num_add_steps = ceil(fabs(additional_angle) / max_angle_step) + 1;
        for(double step=0; step<num_add_steps; step++)
        {
          double frac = step / float(num_add_steps-1);
          Eigen::eigen2_AngleAxis<double> step_rotation(additional_angle*frac, z);
          step_trans = step_rotation * towards_trans;
          for(int i=0; i<3; i++) step_trans(i,3) = towards_trans(i,3);
          tf::poseEigenToMsg(step_trans, desired_pose.pose);      
          desired_pose.header.frame_id = "torso_lift_link";
          desired_pose.header.stamp = ros::Time::now();
          desired_poses.push_back(desired_pose);
        }
      }
    }

    //Gather points from multiple viewpoints while turning the object
    bool result;
    sensor_msgs::PointCloud2 cluster;
    for(size_t view_ind=0; view_ind<desired_poses.size(); view_ind++){

      //move the arm to the next viewpoint
      //std::cout << "moving to desired pose:" << desired_poses[view_ind] << std::endl;
      mech_interface_.moveArmToPoseCartesian(arm_name, desired_poses[view_ind], 
                                             ros::Duration(1.0), .015, .09, .02, .16, .1);

      //point the head
      pointHead(arm_name);
      //ros::Duration(1.0).sleep();

      //get the cluster of points in the hand
      result = callSegmentObjectInHand(arm_name, cluster);
      if(result != 0)
      {
        //combine resulting point cloud into object model
        if(object_in_hand.width == 0) object_in_hand = cluster;
        else
        {
          result = pcl::concatenatePointCloud(object_in_hand, cluster, object_in_hand);
          ROS_INFO("concatenate result: %d", result);
          object_in_hand.header.stamp = ros::Time::now();
        }

        //publish the new result
        ROS_INFO("frame_id: %s, num points: %d", object_in_hand.header.frame_id.c_str(), (int)object_in_hand.width);
        pub_cloud.publish(object_in_hand);
      }
    }

    return 1;
  }


  bool moveArmAway(std::string arm_name)
  {
    geometry_msgs::PoseStamped current_pose = mech_interface_.getGripperPose(arm_name, std::string("base_link"));
    geometry_msgs::Vector3Stamped translation;
    translation.header.frame_id = "base_link";
    translation.header.stamp = ros::Time::now();
    translation.vector.z = .01;
    mech_interface_.translateGripperCartesian(arm_name, translation, ros::Duration(3.0), .015, .09, .02, .16, .1);
    return 1;
  }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "create_object_model_server");
  InHandObjectModeler node;
  ROS_INFO("create object model action server ready");
  ros::spin();
  return 0;
}

---

pr2_create_object_model
Author(s): Kaijen Hsiao
autogenerated on Fri Jan 11 09:35:25 2013