create_object_model_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Author(s): Kaijen Hsiao
00031 
00032 #include <ros/ros.h>
00033 #include <actionlib/server/simple_action_server.h>
00034 #include <sensor_msgs/PointCloud2.h>
00035 #include <sensor_msgs/point_cloud_conversion.h>
00036 #include <tf/transform_listener.h>
00037 #include <pcl/io/io.h>
00038 #include "tabletop_object_detector/SegmentObjectInHand.h"
00039 #include "pr2_create_object_model/ModelObjectInHandAction.h"
00040 #include "pr2_create_object_model/ObjectInHand.h"
00041 #include "object_manipulator/tools/mechanism_interface.h"
00042 #include "arm_navigation_msgs/LinkPadding.h"
00043 #include "arm_navigation_msgs/OrderedCollisionOperations.h"
00044 #include <eigen_conversions/eigen_msg.h>
00045 #include <Eigen/Core>
00046 #include <Eigen/Geometry>
00047 #include <object_manipulation_msgs/ClusterBoundingBox.h>
00048 
00049 #include <tabletop_collision_map_processing/collision_map_interface.h>
00050 
00051 using pr2_create_object_model::ObjectInHand;
00052 
00053 class InHandObjectModeler
00054 {
00055  
00056 private:
00057 
00059   ros::NodeHandle root_nh_;
00060   
00062   ros::NodeHandle priv_nh_;
00063 
00065   actionlib::SimpleActionServer<pr2_create_object_model::ModelObjectInHandAction> model_object_action_server_;
00066 
00068   pr2_create_object_model::ModelObjectInHandResult model_object_result_;
00069   pr2_create_object_model::ModelObjectInHandFeedback model_object_feedback_;
00070 
00072   ros::Publisher pub_cloud;
00073   ros::Publisher pub_pc2;
00074 
00076   std::string segment_object_service_name_;
00077 
00079   object_manipulator::MechanismInterface mech_interface_;
00080 
00082   tabletop_collision_map_processing::CollisionMapInterface collision_map_interface_;
00083 
00085   bool stereo_;
00086 
00087 public:
00088   
00089   InHandObjectModeler(std::string name) : 
00090     root_nh_(""),
00091     priv_nh_("~"),
00092     model_object_action_server_(root_nh_, name+"/model_object_in_hand_action", 
00093                                 boost::bind(&InHandObjectModeler::executeCB, this, _1), false)
00094   {
00095     segment_object_service_name_ = "/segment_object_in_hand_srv";
00096 
00097     while ( !ros::service::waitForService(segment_object_service_name_, ros::Duration().fromSec(3.0)) && priv_nh_.ok() )
00098     {
00099       ROS_INFO("Waiting for service %s...", segment_object_service_name_.c_str());
00100     }
00101     if (!priv_nh_.ok()) exit(0);
00102 
00103     pub_cloud = priv_nh_.advertise<ObjectInHand>("modeled_object_in_hand", 1);
00104     pub_pc2 = priv_nh_.advertise<sensor_msgs::PointCloud2>("modeled_object_in_hand_cloud", 1);
00105 
00106     priv_nh_.param<bool>("stereo", stereo_, true);
00107 
00108     model_object_action_server_.start();
00109     ROS_INFO("started action server");
00110   }
00111 
00113   void executeCB(const pr2_create_object_model::ModelObjectInHandGoalConstPtr &goal)
00114   {
00115     sensor_msgs::PointCloud2 object_in_hand;
00116     sensor_msgs::PointCloud2 cluster;
00117 
00118     //Set the min_disparity high so we can see stuff closer to the camera
00119     if(stereo_)
00120     {
00121       int res = system("rosrun dynamic_reconfigure dynparam set_from_parameters /narrow_stereo_textured/narrow_stereo_textured_proc _min_disparity:=30");
00122     }    
00123     bool result;
00124 
00125     //broadcast the phase we're in
00126     model_object_feedback_.phase = model_object_feedback_.BEFORE_MOVE;
00127     model_object_action_server_.publishFeedback(model_object_feedback_);
00128 
00129     //not supposed to move the arm--just grab any points we see in the hand and return them
00130     if(goal->clear_move.vector.x == 0 && goal->clear_move.vector.y == 0 && goal->clear_move.vector.z == 0){
00131       result = callSegmentObjectInHand(goal->arm_name, cluster);
00132       if(result != 0) object_in_hand = cluster;
00133     }
00134     else
00135     {
00136       //move the arm up to get it away from stuff
00137       moveArmAway(goal->arm_name, goal->clear_move);
00138 
00139       //snap the current points in the hand for motion planning
00140       pointHead(goal->arm_name);
00141       ros::Duration(0.5).sleep();
00142       result = callSegmentObjectInHand(goal->arm_name, cluster);
00143       if(result != 0) object_in_hand = cluster;
00144       
00145       //if desired, move the arm to where the camera can see it
00146       if(goal->rotate_pose.pose.position.x != 0 || goal->rotate_pose.pose.position.y != 0 
00147          || goal->rotate_pose.pose.position.z != 0)
00148       {
00149         moveArmIntoView(goal->arm_name, object_in_hand, goal->rotate_pose);
00150       }
00151 
00152       //if desired, rotate the object, gathering clusters as we go 
00153       if(goal->rotate_object)
00154       { 
00155         //clear the old cluster first
00156         object_in_hand.data.clear();
00157         object_in_hand.width = 0;
00158 
00159         //accumulate a new object model while rotating
00160         if(goal->keep_level) turnArmInwards(goal->arm_name, object_in_hand);
00161         else rotateObject(goal->arm_name, object_in_hand);
00162       }
00163 
00164       //don't rotate, just snap the object where it is
00165       else
00166       {
00167         result = callSegmentObjectInHand(goal->arm_name, cluster);
00168         if(result != 0) object_in_hand = cluster;
00169       }
00170     }
00171 
00172     //broadcast the phase we're in
00173     model_object_feedback_.phase = model_object_feedback_.DONE;
00174     model_object_action_server_.publishFeedback(model_object_feedback_);
00175 
00176     //return the resulting object cluster
00177     model_object_result_.cluster = object_in_hand;
00178     std::cout << "object_in_hand width: " << object_in_hand.width << "\n";
00179     
00180     //if desired, attach the bounding box of the resulting cluster to the gripper in the collision map
00181     std::string collision_name;
00182     if (goal->add_to_collision_map)
00183     {
00184       attachObject(goal->arm_name, object_in_hand, collision_name);
00185     }
00186 
00187     //publish the final result
00188     ObjectInHand oih;
00189     oih.cluster = object_in_hand;
00190     oih.arm_name = goal->arm_name;
00191     oih.collision_name = collision_name;
00192     model_object_result_.collision_name = collision_name;
00193     ROS_INFO("frame_id: %s, final num points: %d, collision name: %s", 
00194              object_in_hand.header.frame_id.c_str(), (int)object_in_hand.width, collision_name.c_str());
00195     pub_cloud.publish(oih);
00196     pub_pc2.publish(oih.cluster);
00197 
00198     //Set the min_disparity back
00199     if(stereo_)
00200     {
00201       int res = system("rosrun dynamic_reconfigure dynparam set_from_parameters /narrow_stereo_textured/narrow_stereo_textured_proc _min_disparity:=0");
00202     }
00203     model_object_action_server_.setSucceeded(model_object_result_);
00204   }
00205 
00207   bool attachObject(std::string arm_name, sensor_msgs::PointCloud2 object_in_hand, std::string &collision_name)
00208   {
00209     if(object_in_hand.width == 0){
00210       ROS_ERROR("object_in_hand was empty!");
00211       return false;
00212     }
00213 
00214     try
00215     {
00216       ros::Time start_time = ros::Time::now();
00217       while (!collision_map_interface_.connectionsEstablished(ros::Duration(1.0))) 
00218       {
00219         if (!priv_nh_.ok() || ros::Time::now() - start_time >= ros::Duration(5.0)) 
00220         {
00221           ROS_ERROR("Create object model: failed to connect to collision map interface!");
00222           collision_name.clear();
00223           return false;
00224         }
00225       }
00226 
00227       //remove any current object in the hand from the collision map
00228       mech_interface_.detachAllObjectsFromGripper(arm_name);
00229 
00230       //convert the object cluster to PointCloud1
00231       //sensor_msgs::PointCloud cluster1;
00232       //sensor_msgs::convertPointCloud2ToPointCloud(object_in_hand, cluster1);
00233       //collision_map_interface_.processCollisionGeometryForCluster(cluster1, collision_name);
00234             
00235       //compute the cluster bounding box and add it to collision map
00236       object_manipulation_msgs::ClusterBoundingBox bbox;
00237       collision_map_interface_.getClusterBoundingBox3D(object_in_hand, bbox.pose_stamped, bbox.dimensions);
00238       //collision_map_interface_.getClusterBoundingBox(cluster1, bbox.pose_stamped, bbox.dimensions);
00239       collision_map_interface_.processCollisionGeometryForBoundingBox(bbox, collision_name);
00240       
00241       //give the cluster collision object time to instantiate
00242       ros::Duration(1.0).sleep();
00243 
00244       //attach the box to the gripper
00245       mech_interface_.attachObjectToGripper(arm_name, collision_name);
00246     }
00247     catch(...)
00248     {
00249       ROS_ERROR("Error while trying to add object to collision map");
00250       collision_name.clear();
00251       return false;
00252     }
00253     return true;
00254   }
00255 
00256 
00258   bool callSegmentObjectInHand(std::string arm_name, sensor_msgs::PointCloud2 &object_in_hand)
00259   {
00260     tabletop_object_detector::SegmentObjectInHand segmentation_srv;
00261     if (arm_name.compare("right_arm") == 0)
00262       segmentation_srv.request.wrist_frame = std::string("r_wrist_roll_link");
00263     else
00264       segmentation_srv.request.wrist_frame = std::string("l_wrist_roll_link");
00265     if (!ros::service::call(segment_object_service_name_, segmentation_srv))
00266     {
00267       ROS_ERROR("Call to segmentation service failed");
00268       return 0;
00269     }
00270     object_in_hand = segmentation_srv.response.cluster;
00271     return 1;
00272   }
00273 
00275   void pointHead(std::string arm_name)
00276   {
00277     geometry_msgs::PointStamped desired_head_point;
00278     if (arm_name.compare("right_arm") == 0)
00279       desired_head_point.header.frame_id = "r_wrist_roll_link";
00280     else
00281       desired_head_point.header.frame_id = "l_wrist_roll_link";
00282     desired_head_point.point.x = .2;
00283     if(stereo_) mech_interface_.pointHeadAction(desired_head_point, "/narrow_stereo_optical_frame");
00284     else mech_interface_.pointHeadAction(desired_head_point, "/openni_rgb_optical_frame");
00285     /* else
00286     {
00287       geometry_msgs::PoseStamped gripper_pose = mech_interface_.getGripperPose(arm_name, std::string("torso_lift_link"));
00288       desired_head_point.header.frame_id = "torso_lift_link";
00289       desired_head_point.point.x = gripper_pose.pose.position.x;
00290       desired_head_point.point.y = gripper_pose.pose.position.y;
00291       desired_head_point.point.z = gripper_pose.pose.position.z;
00292       mech_interface_.pointHeadAction(desired_head_point, "/openni_rgb_optical_frame");
00293     }*/
00294     ROS_INFO("pointing head");
00295   }
00296 
00298   bool moveArmIntoView(std::string arm_name, sensor_msgs::PointCloud2 object_in_hand, geometry_msgs::PoseStamped desired_pose)
00299   {
00300     /*
00301     geometry_msgs::PoseStamped desired_pose = mech_interface_.getGripperPose(arm_name, std::string("torso_lift_link"));
00302     desired_pose.pose.position.x = .7;
00303     desired_pose.pose.position.y = 0;
00304     desired_pose.pose.position.z = 0.40;
00305     */
00306 
00307     //broadcast the phase we're in
00308     model_object_feedback_.phase = model_object_feedback_.MOVE_TO_ROTATE_POSE;
00309     model_object_action_server_.publishFeedback(model_object_feedback_);
00310 
00311     //if there are any points in object_in_hand, attach them to the gripper
00312     std::string collision_name;
00313     bool result = false;
00314     attachObject(arm_name, object_in_hand, collision_name);
00315     
00316     // //try to get to the desired pose using move_arm
00317     // const arm_navigation_msgs::OrderedCollisionOperations col_opers;
00318     // std::vector<arm_navigation_msgs::LinkPadding> link_padding;
00319     // bool result = mech_interface_.moveArmToPose(arm_name, desired_pose, col_opers, link_padding);
00320 
00321     //detach the object from the gripper
00322     mech_interface_.detachAllObjectsFromGripper(arm_name);
00323 
00324     //use Cartesian controllers to go straight there in the event of failure (and raise the elbow)
00325     if(!result)
00326     {
00327       geometry_msgs::PoseStamped current_pose = mech_interface_.getGripperPose(arm_name, desired_pose.header.frame_id);
00328       double pos_dist, angle;
00329       mech_interface_.poseDists(current_pose.pose, desired_pose.pose, pos_dist, angle);
00330       double time = pos_dist / .10;
00331       double angle_time = angle / .785;
00332       if (angle_time > time) time = angle_time;
00333       const double left_elbow_up_angles[7] = {0.79,0,1.6,9999,9999,9999,9999};
00334       const double right_elbow_up_angles[7] = {-0.79,0,-1.6,9999,9999,9999,9999};
00335       std::vector<double> left_elbow_up_angles_vect(left_elbow_up_angles, left_elbow_up_angles+7); 
00336       std::vector<double> right_elbow_up_angles_vect(right_elbow_up_angles, right_elbow_up_angles+7);
00337       if(arm_name == "left_arm") mech_interface_.sendCartesianPostureCommand(arm_name, left_elbow_up_angles_vect);
00338       else mech_interface_.sendCartesianPostureCommand(arm_name, right_elbow_up_angles_vect);
00339       mech_interface_.moveArmToPoseCartesian(arm_name, desired_pose, ros::Duration(time + 2.0), .015, .09, .04, .3, .1);
00340     }
00341     ROS_INFO("moving arm in front of head");
00342 
00343     //point the head at the gripper and wait for it to get there
00344     pointHead(arm_name);
00345     ros::Duration(0.5).sleep();
00346 
00347     return 1;
00348   }
00349 
00351   bool rotateObject(std::string arm_name, sensor_msgs::PointCloud2 &object_in_hand)
00352   {
00353     sensor_msgs::PointCloud2 cluster;
00354     ROS_INFO("inside rotateObject");
00355     
00356     //get the current arm angles
00357     std::vector<double> current_arm_angles;
00358     mech_interface_.getArmAngles(arm_name, current_arm_angles);
00359 
00360     //rotate the gripper by turning the last joint
00361     double max_angle_step = M_PI/2;
00362     int num_steps = 2*M_PI/max_angle_step;
00363     double step_size = 2*M_PI/num_steps;
00364     for(int view_ind=0; view_ind<num_steps; view_ind++)
00365     {
00366       //ask the arm to move to the new joint angles (except for the first step, which is the current pose)
00367       if(view_ind != 0)
00368       {
00369         std::vector< std::vector<double> > trajectory;
00370         trajectory.push_back(current_arm_angles);
00371         current_arm_angles[6] += step_size;
00372         trajectory.push_back(current_arm_angles);
00373         mech_interface_.attemptTrajectory(arm_name, trajectory, false, 0.75);
00374       }
00375 
00376       //point the head at the object in the gripper
00377       pointHead(arm_name);
00378       //ros::Duration(1.0).sleep();
00379 
00380       //broadcast the step we're on
00381       model_object_feedback_.phase = model_object_feedback_.ROTATING;
00382       model_object_feedback_.rotate_ind = view_ind;
00383       model_object_action_server_.publishFeedback(model_object_feedback_);      
00384       
00385       //get the cluster of points in the hand
00386       bool result = callSegmentObjectInHand(arm_name, cluster);
00387       if(result != 0)
00388       {
00389         //combine resulting point cloud into object model
00390         if(object_in_hand.width == 0) object_in_hand = cluster;
00391         else
00392         {
00393           result = pcl::concatenatePointCloud(object_in_hand, cluster, object_in_hand);
00394           object_in_hand.header.stamp = ros::Time::now();
00395         }
00396         
00397         //publish the new result
00398         ROS_INFO("frame_id: %s, num points: %d", object_in_hand.header.frame_id.c_str(), (int)object_in_hand.width);
00399         //pub_pc2.publish(object_in_hand);
00400       }
00401     }
00402     return 1;
00403   }
00404 
00406   bool turnArmInwards(std::string arm_name, sensor_msgs::PointCloud2 &object_in_hand)
00407   {
00408 
00409     geometry_msgs::PoseStamped current_pose = mech_interface_.getGripperPose(arm_name, "torso_lift_link");
00410     Eigen::Affine3d current_trans;
00411     tf::poseMsgToEigen(current_pose.pose, current_trans);
00412     Eigen::Affine3d desired_trans;
00413 
00414     int num_steps = 0;
00415     double rot_angle;
00416     std::vector<geometry_msgs::PoseStamped> desired_poses;
00417     Eigen::Vector3d z(0,0,1);
00418 
00419     //need to keep the object level--compute desired poses that try to turn the gripper inwards
00420     //find the angle that rotates the current gripper x-axis to the torso_lift_link x-z plane, pointed in the -x direction
00421     //but make the gripper swing towards the center 
00422     //(for the right arm, -PI/4 moves +3PI/4, left arm reversed)
00423     Eigen::Vector3d x_axis = current_trans.rotation().col(0);
00424     rot_angle = atan2(x_axis[1], -x_axis[0]);
00425     //std::cout << "rot_angle before:" << rot_angle << std::endl;
00426     if(arm_name.compare("right_arm") == 0 && rot_angle < -M_PI/4) rot_angle += 2*M_PI;
00427     if(arm_name.compare("left_arm") == 0 && rot_angle > M_PI/4) rot_angle -= 2*M_PI; 
00428     //std::cout << "x_axis:\n" << x_axis << std::endl;
00429     std::cout << "rot_angle after:" << rot_angle << std::endl;
00430 
00431     //compute the number of steps based on the max angle step to move before trying to snap a point cloud
00432     double max_angle_step = M_PI/2;
00433     num_steps = ceil(fabs(rot_angle) / max_angle_step) + 1;
00434     //std::cout << "num_steps:" << num_steps << std::endl;
00435 
00436     //compute the sequence of desired poses at which to snap point clouds by interpolating
00437     Eigen::Affine3d step_trans; 
00438     geometry_msgs::PoseStamped desired_pose;
00439     for(int view_ind=0; view_ind<num_steps; view_ind++)  
00440     {
00441       double frac;
00442       if(num_steps == 1) frac = 1;
00443       else frac = view_ind / float(num_steps-1);
00444       //std::cout << "frac:" << frac << std::endl;
00445       Eigen::AngleAxis<double> step_rotation(rot_angle*frac, z);
00446       step_trans = step_rotation * current_trans;
00447       for(int i=0; i<3; i++) step_trans(i,3) = current_trans(i,3);
00448       tf::poseEigenToMsg(step_trans, desired_pose.pose);      
00449       desired_pose.header.frame_id = "torso_lift_link";
00450       desired_pose.header.stamp = ros::Time::now();
00451       desired_poses.push_back(desired_pose);
00452       //std::cout << "desired pose:" << desired_pose << std::endl;
00453     }
00454     Eigen::Affine3d towards_trans = step_trans;
00455     //now go PI towards the center if we haven't gone that far already
00456     if(fabs(rot_angle) < M_PI)
00457     {
00458       double additional_angle = -M_PI;
00459       if(arm_name.compare("left_arm") == 0) additional_angle = M_PI;
00460       int num_add_steps = ceil(fabs(additional_angle) / max_angle_step) + 1;
00461       for(double step=0; step<num_add_steps; step++)
00462       {
00463         double frac = step / float(num_add_steps-1);
00464         Eigen::AngleAxis<double> step_rotation(additional_angle*frac, z);
00465         step_trans = step_rotation * towards_trans;
00466         for(int i=0; i<3; i++) step_trans(i,3) = towards_trans(i,3);
00467         tf::poseEigenToMsg(step_trans, desired_pose.pose);      
00468         desired_pose.header.frame_id = "torso_lift_link";
00469         desired_pose.header.stamp = ros::Time::now();
00470         desired_poses.push_back(desired_pose);
00471       } 
00472     }
00473 
00474     //Gather points from multiple viewpoints while turning the object
00475     bool result;
00476     sensor_msgs::PointCloud2 cluster;
00477     for(size_t view_ind=0; view_ind<desired_poses.size(); view_ind++){
00478       
00479       //move the arm to the next viewpoint
00480       //std::cout << "moving to desired pose:" << desired_poses[view_ind] << std::endl;
00481       mech_interface_.moveArmToPoseCartesian(arm_name, desired_poses[view_ind], 
00482                                              ros::Duration(1.0), .015, .09, .02, .16, .1);
00483       
00484       //point the head
00485       pointHead(arm_name);
00486       //ros::Duration(1.0).sleep();
00487 
00488       //broadcast the step we're on
00489       model_object_feedback_.phase = model_object_feedback_.ROTATING;
00490       model_object_feedback_.rotate_ind = view_ind;
00491       model_object_action_server_.publishFeedback(model_object_feedback_);
00492       
00493       //get the cluster of points in the hand
00494       result = callSegmentObjectInHand(arm_name, cluster);
00495       if(result != 0)
00496       {
00497         //combine resulting point cloud into object model
00498         if(object_in_hand.width == 0) object_in_hand = cluster;
00499         else
00500         {
00501           result = pcl::concatenatePointCloud(object_in_hand, cluster, object_in_hand);
00502           ROS_INFO("concatenate result: %d", result);
00503           object_in_hand.header.stamp = ros::Time::now();
00504         }
00505         
00506         //publish the new result
00507         ROS_INFO("frame_id: %s, num points: %d", object_in_hand.header.frame_id.c_str(), (int)object_in_hand.width);
00508         //pub_pc2.publish(object_in_hand);
00509       }
00510     }
00511     
00512     return 1;
00513   }
00514 
00515 
00517   bool moveArmAway(std::string arm_name, geometry_msgs::Vector3Stamped clear_move)
00518   {
00519     //broadcast the phase we're in
00520     model_object_feedback_.phase = model_object_feedback_.CLEAR_MOVE;
00521     model_object_action_server_.publishFeedback(model_object_feedback_);
00522 
00523     mech_interface_.translateGripperCartesian(arm_name, clear_move, ros::Duration(5.0), .015, .09, .02, .16, .1);
00524     return 1;
00525   }
00526 
00527 };
00528 
00529 int main(int argc, char** argv)
00530 {
00531   ros::init(argc, argv, "create_object_model_server");
00532   InHandObjectModeler node(ros::this_node::getName());
00533   ROS_INFO("create object model action server ready");
00534   ros::spin();
00535   return 0;
00536 }


pr2_create_object_model
Author(s): Kaijen Hsiao
autogenerated on Mon Oct 6 2014 11:49:24