vs_pose.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  *  Copyright (c) 2012, Georgia Institute of Technology
00004  *  All rights reserved.
00005  *
00006  *  Redistribution and use in source and binary forms, with or without
00007  *  modification, are permitted provided that the following conditions
00008  *  are met:
00009  *
00010  *   * Redistributions of source code must retain the above copyright
00011  *     notice, this list of conditions and the following disclaimer.
00012  *   * Redistributions in binary form must reproduce the above
00013  *     copyright notice, this list of conditions and the following
00014  *     disclaimer in the documentation and/or other materials provided
00015  *     with the distribution.
00016  *   * Neither the name of the Georgia Institute of Technology nor the names of
00017  *     its contributors may be used to endorse or promote products derived
00018  *     from this software without specific prior written permission.
00019  *
00020  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  *  POSSIBILITY OF SUCH DAMAGE.
00032  *********************************************************************/
00033 
00034 // ROS
00035 #include <ros/ros.h>
00036 #include <ros/package.h>
00037 
00038 #include <std_msgs/Header.h>
00039 #include <std_msgs/String.h>
00040 #include <geometry_msgs/PointStamped.h>
00041 #include <geometry_msgs/QuaternionStamped.h>
00042 #include <geometry_msgs/TwistStamped.h>
00043 #include <geometry_msgs/Pose2D.h>
00044 #include <geometry_msgs/Point.h>
00045 #include <sensor_msgs/Image.h>
00046 #include <sensor_msgs/PointCloud2.h>
00047 #include <sensor_msgs/CameraInfo.h>
00048 #include <sensor_msgs/image_encodings.h>
00049 
00050 #include <message_filters/subscriber.h>
00051 #include <message_filters/time_synchronizer.h>
00052 #include <message_filters/synchronizer.h>
00053 #include <message_filters/sync_policies/approximate_time.h>
00054 #include <image_transport/image_transport.h>
00055 #include <cv_bridge/cv_bridge.h>
00056 
00057 // TF
00058 #include <tf/transform_listener.h>
00059 #include <tf/transform_datatypes.h>
00060 
00061 // PCL
00062 #include <pcl/point_cloud.h>
00063 #include <pcl/point_types.h>
00064 #include <pcl_ros/transforms.h>
00065 
00066 // Boost
00067 #include <boost/shared_ptr.hpp>
00068 
00069 // OpenCV
00070 #include <opencv2/core/core.hpp>
00071 #include <opencv2/imgproc/imgproc.hpp>
00072 #include <opencv2/highgui/highgui.hpp>
00073 
00074 // STL
00075 #include <vector>
00076 #include <deque>
00077 #include <queue>
00078 #include <string>
00079 #include <sstream>
00080 #include <iostream>
00081 #include <fstream>
00082 #include <utility>
00083 #include <stdexcept>
00084 #include <float.h>
00085 #include <math.h>
00086 #include <time.h> // for srand(time(NULL))
00087 #include <cstdlib> // for MAX_RAND
00088 #include <sstream>
00089 
00090 // PR2_GRIPPER_SENSOR_ACTION
00091 #include <actionlib/client/simple_action_client.h>
00092 #include <actionlib/client/terminal_state.h>
00093 #include <visual_servo/VisualServoAction.h>
00094 
00095 // Others
00096 #include <visual_servo/VisualServoTwist.h>
00097 #include <visual_servo/VisualServoPose.h>
00098 #include <std_srvs/Empty.h>
00099 #include "visual_servo.cpp"
00100 
00101 // statemachine constants
00102 #define INIT          0
00103 #define SETTLE        1
00104 #define INIT_OBJS     2
00105 #define INIT_HAND     3
00106 #define INIT_DESIRED  4
00107 #define POSE_CONTR    5
00108 #define POSE_CONTR_2  6
00109 #define VS_CONTR_1    7
00110 #define VS_CONTR_2    8
00111 #define GRAB          9
00112 #define RELOCATE      10
00113 #define DESCEND_INIT  11
00114 #define DESCEND       12
00115 #define RELEASE       13
00116 #define FINISH        14
00117 #define TERM          15
00118 
00119 // floating point mod
00120 #define fmod(a,b) a - (float)((int)(a/b)*b)
00121 
00122 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image,sensor_msgs::PointCloud2> MySyncPolicy;
00123 typedef pcl::PointCloud<pcl::PointXYZ> XYZPointCloud;
00124 typedef actionlib::SimpleActionClient<visual_servo::VisualServoAction> VisualServoClient; 
00125 
00126 using boost::shared_ptr;
00127 using geometry_msgs::TwistStamped;
00128 using geometry_msgs::PoseStamped;
00129 using geometry_msgs::QuaternionStamped;
00130 using geometry_msgs::Point;
00131 using visual_servo::VisualServoTwist;
00132 using visual_servo::VisualServoPose;
00133 
00134 class VisualServoNode
00135 {
00136   public:
00137     VisualServoNode(ros::NodeHandle &n) :
00138       n_(n), n_private_("~"),
00139       image_sub_(n, "color_image_topic", 1),
00140       depth_sub_(n, "depth_image_topic", 1),
00141       cloud_sub_(n, "point_cloud_topic", 1),
00142       sync_(MySyncPolicy(15), image_sub_, depth_sub_, cloud_sub_),
00143       it_(n), tf_(), have_depth_data_(false), camera_initialized_(false),
00144       PHASE(INIT)
00145   {
00146     vs_ = shared_ptr<VisualServo>(new VisualServo(JACOBIAN_TYPE_PSEUDO));
00147     tf_ = shared_ptr<tf::TransformListener>(new tf::TransformListener());
00148 
00149     n_private_.param("display_wait_ms", display_wait_ms_, 3);
00150 
00151     std::string default_optical_frame = "/head_mount_kinect_rgb_optical_frame";
00152     n_private_.param("optical_frame", optical_frame_, default_optical_frame);
00153     std::string default_workspace_frame = "/torso_lift_link";
00154     n_private_.param("workspace_frame", workspace_frame_, default_workspace_frame);
00155     std::string cam_info_topic_def = "/kinect_head/rgb/camera_info";
00156     n_private_.param("cam_info_topic", cam_info_topic_, cam_info_topic_def);
00157 
00158     // color segmentation parameters
00159     n_private_.param("target_hue_value", target_hue_value_, 10);
00160     n_private_.param("target_hue_threshold", target_hue_threshold_, 20);
00161     n_private_.param("gripper_tape_hue_value", gripper_tape_hue_value_, 180);
00162     n_private_.param("gripper_tape_hue_threshold", gripper_tape_hue_threshold_, 50);
00163     n_private_.param("default_sat_bot_value", default_sat_bot_value_, 40);
00164     n_private_.param("default_sat_top_value", default_sat_top_value_, 40);
00165     n_private_.param("default_val_value", default_val_value_, 200);
00166     n_private_.param("min_contour_size", min_contour_size_, 10.0);
00167 
00168     // others
00169     n_private_.param("jacobian_type", jacobian_type_, JACOBIAN_TYPE_INV);
00170     n_private_.param("vs_err_term_thres", vs_err_term_threshold_, 0.001);
00171     n_private_.param("pose_servo_z_offset", pose_servo_z_offset_, 0.045);
00172     n_private_.param("place_z_velocity", place_z_velocity_, -0.025);
00173     n_private_.param("gripper_tape1_offset_x", tape1_offset_x_, 0.02);
00174     n_private_.param("gripper_tape1_offset_y", tape1_offset_y_, -0.025);
00175     n_private_.param("gripper_tape1_offset_z", tape1_offset_z_, 0.07);
00176 
00177     // Setup ros node connections
00178     sync_.registerCallback(&VisualServoNode::sensorCallback, this);
00179     initializeService();
00180     ROS_INFO("Initialization 0: Node init & Register Callback Done");
00181   }
00182 
00183     // destructor
00184     ~VisualServoNode()
00185     {
00186     }
00187 
00191     void sensorCallback(const sensor_msgs::ImageConstPtr& img_msg, 
00192         const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
00193     {
00194 
00195       ros::Time start = ros::Time::now();
00196 
00197       // Store camera information only once
00198       if (!camera_initialized_)
00199       {
00200         cam_info_ = *ros::topic::waitForMessage<sensor_msgs::CameraInfo>(cam_info_topic_, n_, ros::Duration(3.0));
00201         camera_initialized_ = true;
00202         vs_->setCamInfo(cam_info_);
00203         ROS_INFO("Initialization: Camera Info Done");
00204       }
00205 
00206       cv::Mat color_frame, depth_frame;
00207       cv_bridge::CvImagePtr color_cv_ptr = cv_bridge::toCvCopy(img_msg);
00208       cv_bridge::CvImagePtr depth_cv_ptr = cv_bridge::toCvCopy(depth_msg);
00209 
00210       color_frame = color_cv_ptr->image;
00211       depth_frame = depth_cv_ptr->image;
00212 
00213       XYZPointCloud cloud; 
00214       pcl::fromROSMsg(*cloud_msg, cloud);
00215       tf_->waitForTransform(workspace_frame_, cloud.header.frame_id,
00216           cloud.header.stamp, ros::Duration(0.9));
00217       pcl_ros::transformPointCloud(workspace_frame_, cloud, cloud, *tf_);
00218       cur_camera_header_ = img_msg->header;
00219       cur_color_frame_ = color_frame;
00220       cur_orig_color_frame_ = color_frame.clone();
00221       cur_depth_frame_ = depth_frame.clone();
00222       cur_point_cloud_ = cloud;
00223 
00224       // need to profile this
00225       executeStatemachine();
00226 
00227 #define DISPLAY 1
00228 #ifdef DISPLAY
00229       // show a pretty imshow
00230       // setDisplay();
00231 #endif
00232     }
00233 
00234     void executeStatemachine()
00235     {
00236       temp_draw_.clear();
00237       switch(PHASE)
00238       {
00239         case INIT:
00240           {
00241             ROS_INFO("Initializing Services and Robot Configuration");
00242             std_srvs::Empty e;
00243             i_client_.call(e);
00244             PHASE = SETTLE;
00245           }
00246           break;
00247 
00248         case SETTLE:
00249           {
00250             ROS_INFO("Phase Settle: Move Gripper to Init Position");
00251             // Move Hand to Some Preset Pose
00252             visual_servo::VisualServoPose p_srv = formPoseService(0.62, 0.05, -0.1);
00253             p_srv.request.arm = "l";
00254             if (p_client_.call(p_srv))
00255             {
00256               PHASE = VS_CONTR_1;
00257             }
00258             else
00259             {
00260               ROS_WARN("Failed to put the hand in initial configuration");
00261               ROS_WARN("For debug purpose, just skipping to the next step");
00262               PHASE = VS_CONTR_1;
00263             }
00264           }
00265           break;
00266 
00267         case VS_CONTR_1:
00268           {
00269             // send a goal to the action
00270             visual_servo::VisualServoGoal goal;
00271             PoseStamped p;
00272             p.pose.position.x = 0.55;
00273             p.pose.position.y = 0.2;
00274             p.pose.position.z = 0.05;
00275             goal.pose = p;
00276 
00277             double timeout = 30.0;
00278             ROS_INFO("Sending Goal [%.3f %.3f %.3f] (TO = %.1f)", p.pose.position.x,
00279             p.pose.position.y, p.pose.position.z, timeout);
00280             l_vs_client_->sendGoal(goal);
00281 
00282             bool finished_before_timeout = l_vs_client_->waitForResult(ros::Duration(timeout));
00283 
00284             if (finished_before_timeout)
00285             {
00286               actionlib::SimpleClientGoalState state = l_vs_client_->getState();
00287               ROS_INFO("Action finished: %s",state.toString().c_str());
00288             }
00289           }
00290           break;
00291 
00292         default:
00293           {
00294             // make the list shorter
00295             ROS_INFO("Routine Ended.");
00296             std::cout << "Press [Enter] if you want to do it again: ";
00297             while(!ros::isShuttingDown())
00298             {
00299               int c = std::cin.get();
00300               if (c  == '\n')
00301                 break;
00302             }
00303 
00304             printf("Reset the arm and repeat Pick and Place in 3 seconds\n");
00305             // try to initialize the arm
00306             std_srvs::Empty e;
00307             i_client_.call(e);
00308 
00309           }
00310           break;
00311       }
00312     }
00313 
00314     void setDisplay()
00315     {
00316       cv::imshow("in", cur_orig_color_frame_); 
00317       cv::waitKey(display_wait_ms_);
00318     }
00319 
00320     void initializeService()
00321     {
00322       ROS_DEBUG(">> Hooking Up The Service");
00323       ros::NodeHandle n;
00324       v_client_ = n.serviceClient<visual_servo::VisualServoTwist>("vs_twist");
00325       p_client_ = n.serviceClient<visual_servo::VisualServoPose>("vs_pose");
00326       i_client_ = n.serviceClient<std_srvs::Empty>("vs_init");
00327 
00328       // uncomment below depending on which arm you want
00329       // l_vs_client_ = new VisualServoClient(n_, "r_vs_controller/vsaction");
00330       l_vs_client_ = new VisualServoClient(n_, "l_vs_controller/vsaction");
00331       while(!l_vs_client_->waitForServer(ros::Duration(5.0))){
00332         ROS_INFO("Waiting for Visual Servo action...");
00333       }
00334     }
00335 
00336 
00337     /**********************
00338      * HELPER METHODS
00339      **********************/
00340 
00341     bool sendZeroVelocity()
00342     {
00343       // assume everything is init to zero
00344       visual_servo::VisualServoTwist v_srv;
00345 
00346       // need this to mvoe the arm
00347       v_srv.request.error = 1;
00348       return v_client_.call(v_srv);
00349     }
00350 
00351     visual_servo::VisualServoPose formPoseService(float px, float py, float pz)
00352     {
00353       return VisualServoMsg::createPoseMsg(px, py, pz, -0.4582, 0, 0.8889, 0);
00354     }
00355 
00356     void printMatrix(cv::Mat_<double> in)
00357     {
00358       for (int i = 0; i < in.rows; i++) {
00359         for (int j = 0; j < in.cols; j++) {
00360           printf("%+.5f\t", in(i,j)); 
00361         }
00362         printf("\n");
00363       }
00364     }
00365 
00366 
00370     void spin()
00371     {
00372       while(n_.ok())
00373       {
00374         ros::spinOnce();
00375       }
00376     }
00377 
00378   protected:
00379     ros::NodeHandle n_;
00380     ros::NodeHandle n_private_;
00381     message_filters::Subscriber<sensor_msgs::Image> image_sub_;
00382     message_filters::Subscriber<sensor_msgs::Image> depth_sub_;
00383     message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
00384     message_filters::Synchronizer<MySyncPolicy> sync_;
00385     image_transport::ImageTransport it_;
00386     sensor_msgs::CameraInfo cam_info_;
00387     shared_ptr<tf::TransformListener> tf_;
00388     cv::Mat cur_color_frame_;
00389     cv::Mat cur_orig_color_frame_;
00390     cv::Mat cur_depth_frame_;
00391     cv::Mat cur_workspace_mask_;
00392     std_msgs::Header cur_camera_header_;
00393     XYZPointCloud cur_point_cloud_;
00394 
00395     // visual servo object
00396     shared_ptr<VisualServo> vs_;
00397 
00398     bool have_depth_data_;
00399     int display_wait_ms_;
00400     int num_downsamples_;
00401     std::string workspace_frame_;
00402     std::string optical_frame_;
00403     bool camera_initialized_;
00404     bool desire_points_initialized_;
00405     std::string cam_info_topic_;
00406     int tracker_count_;
00407 
00408     // clients to services provided by vs_controller.py
00409     ros::ServiceClient v_client_;
00410     ros::ServiceClient p_client_;
00411     ros::ServiceClient i_client_;
00412 
00413 
00414     // segmenting
00415     int target_hue_value_;
00416     int target_hue_threshold_;
00417     int gripper_tape_hue_value_;
00418     int gripper_tape_hue_threshold_;
00419     int default_sat_bot_value_;
00420     int default_sat_top_value_;
00421     int default_val_value_;
00422     double min_contour_size_;
00423 
00424     // Other params
00425     int jacobian_type_;
00426     double vs_err_term_threshold_;
00427     double pose_servo_z_offset_;
00428     double place_z_velocity_;
00429     double tape1_offset_x_;
00430     double tape1_offset_y_;
00431     double tape1_offset_z_;
00432 
00433     // State machine variable
00434     unsigned int PHASE;
00435 
00436     // gripper sensor action clients
00437     VisualServoClient* l_vs_client_;
00438     VisualServoClient* r_vs_client_;
00439 
00440     //  drawing
00441     std::vector<cv::Point> temp_draw_;
00442     cv::Rect original_box_;
00443 
00444     // collision detection
00445     bool is_detected_;
00446     bool place_detection_;
00447     float object_z_;
00448 
00449     float close_gripper_dist_;
00450 
00451 };
00452 
00453 int main(int argc, char ** argv)
00454 {
00455   srand(time(NULL));
00456   ros::init(argc, argv, "visual_servo_node");
00457 
00458   log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
00459   my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Info]);
00460 
00461   ros::NodeHandle n;
00462   VisualServoNode vs_node(n);
00463   vs_node.spin();
00464 }


visual_servo
Author(s): Stephan Lee
autogenerated on Wed Nov 27 2013 11:44:03