00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00058 #include <tf/transform_listener.h>
00059 #include <tf/transform_datatypes.h>
00060
00061
00062 #include <pcl/point_cloud.h>
00063 #include <pcl/point_types.h>
00064 #include <pcl_ros/transforms.h>
00065
00066
00067 #include <boost/shared_ptr.hpp>
00068
00069
00070 #include <opencv2/core/core.hpp>
00071 #include <opencv2/imgproc/imgproc.hpp>
00072 #include <opencv2/highgui/highgui.hpp>
00073
00074
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>
00087 #include <cstdlib>
00088 #include <sstream>
00089
00090
00091 #include <actionlib/client/simple_action_client.h>
00092 #include <actionlib/client/terminal_state.h>
00093 #include <visual_servo/VisualServoAction.h>
00094
00095
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
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
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
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
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
00178 sync_.registerCallback(&VisualServoNode::sensorCallback, this);
00179 initializeService();
00180 ROS_INFO("Initialization 0: Node init & Register Callback Done");
00181 }
00182
00183
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
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
00225 executeStatemachine();
00226
00227 #define DISPLAY 1
00228 #ifdef DISPLAY
00229
00230
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
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
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
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
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
00329
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
00339
00340
00341 bool sendZeroVelocity()
00342 {
00343
00344 visual_servo::VisualServoTwist v_srv;
00345
00346
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
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
00409 ros::ServiceClient v_client_;
00410 ros::ServiceClient p_client_;
00411 ros::ServiceClient i_client_;
00412
00413
00414
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
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
00434 unsigned int PHASE;
00435
00436
00437 VisualServoClient* l_vs_client_;
00438 VisualServoClient* r_vs_client_;
00439
00440
00441 std::vector<cv::Point> temp_draw_;
00442 cv::Rect original_box_;
00443
00444
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 }