CalibrationExecutive.cpp
Go to the documentation of this file.
00001 #include <sensor_msgs/PointCloud2.h>
00002 #include <geometry_msgs/Transform.h>
00003 #include <tf/tf.h>
00004 #include <tf_conversions/tf_eigen.h>
00005 
00006 
00007 #include <pcl/ros/conversions.h>
00008 #include <pcl_ros/point_cloud.h>
00009 #include <pcl_ros/transforms.h>
00010 
00011 #include <ethercat_trigger_controllers/SetWaveform.h>
00012 
00013 #include <actionlib/client/simple_action_client.h>
00014 #include <pr2_controllers_msgs/PointHeadAction.h>
00015 
00016 #include <pcl/registration/transformation_estimation_svd.h>
00017 
00018 // ugly test depending on roscpp because tf_conversions is not properly versionized
00019 #if !ROS_VERSION_MINIMUM(1, 9, 30)
00020          #define transformTFToEigen TransformTFToEigen
00021          #define transformEigenToTF TransformEigenToTF
00022 #endif // !ROS_VERSION_MINIMUM(1, 9, 30)
00023 
00024 std::string mount_frame_ = "head_mount_link";
00025 std::string rgb_optical_frame_ = "head_mount_kinect_ir_link";
00026 std::string rgb_topic_ = "/head_mount_kinect/depth_registered/points";
00027 std::string led_frame_ = "r_gripper_led_frame";
00028 
00029 tf::TransformListener*listener_ = 0L;
00030 
00031 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;
00032 
00033 PointHeadClient* point_head_client_;
00034 
00035 void init()
00036 {
00037     if (!listener_)
00038         listener_ = new tf::TransformListener();
00039 }
00040 
00041 tf::Stamped<tf::Pose> getPose(const char target_frame[],const char lookup_frame[], ros::Time tm = ros::Time(0))
00042 {
00043 
00044     init();
00045     ros::Rate rate(100.0);
00046 
00047     tf::StampedTransform transform;
00048     bool transformOk = false;
00049     bool had_to_retry = false;
00050 
00051     //listener_->waitForTransform(target_frame, lookup_frame, tm, ros::Duration(0.5));
00052     while (ros::ok() && (!transformOk))
00053     {
00054         transformOk = true;
00055         try
00056         {
00057             listener_->lookupTransform(target_frame, lookup_frame,tm, transform);
00058         }
00059         catch (tf::TransformException ex)
00060         {
00061             std::string what = ex.what();
00062             what.resize(100);
00063             ROS_INFO("getPose: tf::TransformException ex.what()='%s', will retry",what.c_str());
00064             transformOk = false;
00065             had_to_retry = true;
00066             //listener_->waitForTransform(target_frame, lookup_frame, ros::Time(0), ros::Duration(0.5));
00067         }
00068         if (!transformOk)
00069             rate.sleep();
00070     }
00071 
00072     if (had_to_retry)
00073         ROS_INFO("Retry sucessful");
00074 
00075     tf::Stamped<tf::Pose> ret;
00076     ret.frame_id_ = transform.frame_id_;
00077     ret.stamp_ = transform.stamp_;
00078     ret.setOrigin(transform.getOrigin());
00079     ret.setRotation(transform.getRotation());
00080 
00081     return ret;
00082 }
00083 
00084 void getCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string frame_id, ros::Time after, ros::Time *tm)
00085 {
00086 
00087     sensor_msgs::PointCloud2 pc;
00088     bool found = false;
00089     while (!found)
00090     {
00091         pc  = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>(rgb_topic_));
00092         if ((after == ros::Time(0,0)) || (pc.header.stamp > after))
00093             found = true;
00094         else
00095         {
00096             //ROS_ERROR("getKinectCloudXYZ cloud too old : stamp %f , target time %f",pc.header.stamp.toSec(), after.toSec());
00097         }
00098     }
00099     if (tm)
00100         *tm = pc.header.stamp;
00101 
00102     tf::Stamped<tf::Pose> net_stamped = getPose(rgb_optical_frame_.c_str(),pc.header.frame_id.c_str());
00103     tf::Transform net_transform;
00104     net_transform.setOrigin(net_stamped.getOrigin());
00105     net_transform.setRotation(net_stamped.getRotation());
00106 
00107     sensor_msgs::PointCloud2 pct; //in map frame
00108 
00109     pcl_ros::transformPointCloud(frame_id.c_str(),net_transform,pc,pct);
00110     pct.header.frame_id = frame_id.c_str();
00111 
00112     geometry_msgs::Transform t_msg;
00113     tf::transformTFToMsg(net_transform, t_msg);
00114 
00115     //std::cout << "CLOUD transf" << pc.header.frame_id << " to " << pct.header.frame_id << " : " << t_msg << std::endl;
00116 
00117     pcl::fromROSMsg(pct, *cloud);
00118 }
00119 
00120 
00121 void lookAt(std::string frame_id, double x, double y, double z)
00122 {
00123     //the goal message we will be sending
00124     pr2_controllers_msgs::PointHeadGoal goal;
00125 
00126     //the target point, expressed in the requested frame
00127     geometry_msgs::PointStamped point;
00128     point.header.frame_id = frame_id;
00129     point.point.x = x;
00130     point.point.y = y;
00131     point.point.z = z;
00132     goal.target = point;
00133 
00134     //we are pointing the wide_stereo camera frame
00135     //(pointing_axis defaults to X-axis)
00136     goal.pointing_frame = rgb_optical_frame_;
00137 
00138     //take at least 5 seconds to get there
00139     goal.min_duration = ros::Duration(2);
00140 
00141     //and go no faster than 0.1 rad/s
00142     goal.max_velocity = 0.5;
00143 
00144     //send the goal
00145     point_head_client_->sendGoal(goal);
00146 
00147     point_head_client_->waitForResult();
00148     //std::cout << "lookat finished" << std::endl;
00149 }
00150 
00151 ros::ServiceClient client;
00152 ethercat_trigger_controllers::SetWaveform waveform;
00153 
00154 std::vector<double> field;
00155 
00156 tf::Vector3 locate_led()
00157 {
00158 
00159     std::cout << "Locating LED in Kinect Pointcloud :";
00160 
00161     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00162     pcl::PointCloud<pcl::PointXYZRGB>::Ptr last_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00163 
00164     bool have_last_cloud = false;
00165 
00166     field.resize(640*480*2);
00167     for (std::vector<double>::iterator it= field.begin(); it!= field.end(); ++it)
00168         *it = 0;
00169 
00170     double field_max = -1000;
00171 
00172     tf::Vector3 best_pt;
00173 
00174     double field_avg = 1;
00175 
00176     size_t num_pics = 0;
00177 
00178     // trigger a few times until we have enough evidence we found the led
00179     while (ros::ok() && ((field_max<2500) || (field_max < fabs(field_avg) * 100)))
00180     {
00181 
00182         if (waveform.request.active_low)
00183             waveform.request.active_low = 0;
00184         else
00185             waveform.request.active_low = 1;
00186 
00187         double sign = waveform.request.active_low == 1 ? 1 : -1;
00188 
00189         if (client.call(waveform))
00190         {
00191             //ROS_INFO("CALLED %f ", sign);
00192         }
00193 
00194         ros::Duration(0.01).sleep(); // led should be switched instantly but lets be safe and give it some time to 'settle'
00195         ros::Time lookup_time;
00196 
00197         *last_cloud = *cloud;
00198 
00199         getCloud(cloud, rgb_optical_frame_, ros::Time::now(), &lookup_time);
00200 
00201         num_pics++;
00202 
00203         size_t max_index = 0;
00204         field_avg = 0;
00205 
00206         if (have_last_cloud) {
00207             for (size_t k = 0; k < std::min(last_cloud->points.size(), cloud->points.size()); k++)
00208             {
00209                 field[k] += ((cloud->points[k].r - last_cloud->points[k].r) +
00210                              (cloud->points[k].g - last_cloud->points[k].g) +
00211                              (cloud->points[k].b - last_cloud->points[k].b))  * sign;
00212                 field_avg += fabs(field[k]);
00213                 if (field[k]>field_max) {
00214                     field_max = field[k];
00215                     max_index = k;
00216                 }
00217             }
00218             field_avg /= cloud->points.size();
00219             tf::Vector3 max_pt(cloud->points[max_index].x, cloud->points[max_index].y, cloud->points[max_index].z);
00220             best_pt = max_pt;
00221             //std::cout << "max pt idx" << max_index << " (" << field_max << "):" << "field abs avg " << field_avg << std::endl;
00222             //max_pt.x() << " " << max_pt.y() << " " << max_pt.z() << std::endl;
00223             //std::cout << "field avg " << field_avg << std::endl;
00224         } else {
00225             have_last_cloud = true;
00226         }
00227 
00228         if (field_avg > 0)
00229             std::cout << ".." << std::min((double)100.0,std::min(100.0 * field_max / 2500 , 100.0 * field_max / (fabs(field_avg) * 100))) << "%";
00230 
00231         std::cout.flush();
00232 
00233         if (num_pics > 20)
00234         {
00235             ROS_WARN("It takes unusually long to detect the LED in the kinect frame.");
00236             ROS_WARN("Make sure the right gripper LED is blinking and visible by the kinect - did you start the led controller? Try to keep the background static. Check for possible specular reflections in the kinect rgb image and get rid of them (ceiling lamps etc).");
00237         }
00238 
00239         if (num_pics > 50) {
00240             ROS_WARN("Starting over");
00241             return locate_led();
00242         }
00243 
00244         //rt.sleep();
00245     }
00246 
00247     std::cout << std::endl;
00248     return best_pt;
00249 }
00250 
00251 void find_correspondance(tf::Vector3 &led_k, tf::Vector3 &led_tf)
00252 {
00253         led_k = locate_led();
00254         tf::Stamped<tf::Pose> led_tf_pose = getPose(rgb_optical_frame_.c_str(),led_frame_.c_str());
00255         led_tf = led_tf_pose.getOrigin();
00256         if (!ros::ok())
00257             exit(0);
00258 
00259         std::cout << "Found point correspondance: \n" <<
00260                      "in kinect [ " << led_k.x() << " ," << led_k.y() << " , " << led_k.z() << " ] \n" <<
00261                      "in tf     [ " << led_tf.x() << " ," << led_tf.y() << " , " << led_tf.z() << " ] \n" << std::endl;
00262 }
00263 
00264 int main(int argc,char **argv)
00265 {
00266 
00267     ros::init(argc, argv, "led_calibration_executive");
00268     ros::NodeHandle nh;
00269 
00270     init();
00271 
00272     client = nh.serviceClient<ethercat_trigger_controllers::SetWaveform>("/r_gripper_led/set_waveform");
00273     //ethercat_trigger_controllers::SetWaveform waveform;
00274     waveform.request.rep_rate = 1;
00275     waveform.request.phase = 0;
00276     waveform.request.duty_cycle = 0.5;
00277     waveform.request.running = 0;
00278     waveform.request.active_low = 0;
00279     waveform.request.pulsed = 0;
00280 
00281     point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
00282 
00283     while (!point_head_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
00284     {
00285         ROS_INFO("Waiting for the point_head_action server to come up");
00286     }
00287 
00288 
00289     ros::Rate rt(2);
00290 
00291     std::vector<double> field;
00292     field.resize(640*480*2);
00293     for (std::vector<double>::iterator it= field.begin(); it!= field.end(); ++it)
00294         *it = 0;
00295 
00296     pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> transformation_estimator;
00297     pcl::PointCloud<pcl::PointXYZ> points_in_kinect;
00298     pcl::PointCloud<pcl::PointXYZ> points_in_tf;
00299 
00300     double angle = 0;
00301     while (ros::ok())
00302     {
00303         angle += 37.0 / 180.0 * M_PI;
00304         double radius = .2;
00305         std::cout << "Sent head pointing command, waiting for head to settle ..";
00306         lookAt(led_frame_.c_str(), radius * sin(angle), radius * cos(angle),0);
00307         //std::cout << angle << " " << radius * sin(angle) << " " <<  radius * cos(angle) << std::endl;
00308 
00309         // wait for the head turning to finish
00310         //  ros::Duration(5).sleep();
00311         ros::Rate rt(10);
00312         ros::Time start = ros::Time::now();
00313         tf::Stamped<tf::Pose> hand_pose = getPose(rgb_optical_frame_.c_str(),led_frame_.c_str());
00314         tf::Stamped<tf::Pose> last_hand_pose;
00315         double position_change = 5;
00316         size_t num_settled = 0;
00317         // wait for head vs hand tf to settle as the head should not move during calib
00318 
00319         std::cout.flush();
00320         while  (num_settled < 3) {
00321             rt.sleep();
00322             last_hand_pose = hand_pose;
00323             hand_pose = getPose(rgb_optical_frame_.c_str(),led_frame_.c_str());
00324             //std::cout << "time " << (ros::Time::now() - start).toSec() << " pos " << (hand_pose.getOrigin() - last_hand_pose.getOrigin()).length() <<
00325               //  " angle " << hand_pose.getRotation().angle(last_hand_pose.getRotation()) << std::endl;
00326             position_change = (hand_pose.getOrigin() - last_hand_pose.getOrigin()).length();
00327             if (position_change < 0.0001)
00328                 num_settled++;
00329             else
00330                 num_settled = 0;
00331         }
00332         std::cout <<"..done." << std::endl;
00333 
00334         tf::Vector3 led_tf, led_kinect;
00335         find_correspondance(led_kinect, led_tf);
00336         pcl::PointXYZ pt_k(led_kinect.x(),led_kinect.y(),led_kinect.z());
00337         pcl::PointXYZ pt_tf(led_tf.x(),led_tf.y(),led_tf.z());
00338 
00339         if (led_kinect.x() != led_kinect.x()) {
00340             ROS_WARN("Last point had nan position, skipping it");
00341         } else {
00342             points_in_kinect.points.push_back(pt_k);
00343             points_in_tf.points.push_back(pt_tf);
00344         }
00345 
00346         tf::Stamped<tf::Pose> kinect_pose = getPose(mount_frame_.c_str() ,rgb_optical_frame_.c_str());
00347         tf::Transform kinect_trans = kinect_pose;
00348 
00349         if (points_in_tf.points.size() > 2) {
00350             Eigen::Matrix4f trans;
00351             transformation_estimator.estimateRigidTransformation(points_in_kinect,points_in_tf, trans);
00352 
00353             Eigen::Matrix4d md(trans.cast<double>());
00354             Eigen::Affine3d affine(md);
00355             tf::Transform transform;
00356             tf::transformEigenToTF(affine, transform);
00357 
00358             geometry_msgs::Transform ps;
00359             tf::transformTFToMsg(transform,ps);
00360             std::cout << "Correction relative to current kinect pose:"<< std::endl;
00361             std::cout << ps;
00362             std::cout << "relative angle: " << transform.getRotation().getAngle() << " rad or " <<  transform.getRotation().getAngle() * 180.0f / M_PI << " degrees " << std::endl << std::endl;
00363 
00364             //geometry_msgs::Transform ps_kinect;
00365             //tf::transformTFToMsg(kinect_trans,ps_kinect);
00366             //std::cout << "kinect pose " << ps_kinect << std::endl;
00367 
00368             tf::Transform post = kinect_trans * transform;
00369 
00370             geometry_msgs::Transform ps_post;
00371             tf::transformTFToMsg(post,ps_post);
00372             std::cout << "New pose of " <<  rgb_optical_frame_  << " in " << mount_frame_ << " \n " << ps_post << std::endl;
00373 
00374             btQuaternion q;
00375             double roll, pitch, yaw;
00376             tf::Matrix3x3(post.getRotation()).getRPY(roll, pitch, yaw);
00377             std::cout << "RPY " << roll << " " << pitch << " " << yaw <<  std::endl;
00378 
00379             std::cout << "<origin rpy=\"" << roll << " " << pitch << " " << yaw << "\" xyz=\"" << ps_post.translation.x << " " << ps_post.translation.y << " " << ps_post.translation.z << "\"/> " << std::endl;
00380         }
00381     }
00382 
00383 
00384 }
00385 
00386 
00387 //rosservice call /r_gripper_led/set_waveform '{rep_rate: 1, phase: 0, duty_cycle: 0.5, running: 0, active_low: 0, pulsed: 0}'
00388 
00389 //rosservice call /l_gripper_led/set_waveform '{rep_rate: 1, phase: 0, duty_cycle: 0.5, running: 0, active_low: 0, pulsed: 0}'
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pr2_led_kinect_calib
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 18:54:22