From: <tum...@li...> - 2012-02-10 13:18:50
|
Revision: 991 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=991&view=rev Author: moesenle Date: 2012-02-10 13:18:43 +0000 (Fri, 10 Feb 2012) Log Message: ----------- Author: Thomas Ruehr <ru...@in...> Date: Fri Feb 10 14:18:23 2012 +0100 overhauled for roboearth workshop. commit 9d1ab6e850f0e8fb1e497e691c686817c3b5f2f6 Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 26 19:16:32 2011 +0200 some demo scripting commit 1d198d168e12c3b68143cbafc09e611430e98504 Author: Thomas Ruehr <ru...@in...> Date: Wed Apr 20 11:16:35 2011 +0200 Republishes a PC2 on another topic, e.g. FreezeCloud2 /kinect/cloud_throttled /sample, this is useful for being able to deselect points in rviz (ctrl-klick/drag), since you cant deselect something that is not displayed anymore commit 6d63ae310c09135346b8b918477418f917e66d5d Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 19 09:23:59 2011 +0200 all play and no work commit bba7b8cc4de9981b34436402c3132fb85e6f449f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 31 10:38:18 2011 +0200 SnapMap->Diamondback(eigen namespace), OperatehandleController now extrapolates relative transform from start to current by 5cm to find next hand pose; added static tf publisher to provide fake odom for amcl when wheels are dead commit dc0014e9b51398cfd690f578b7e2d974c4b1b17e Author: Dejan Pangercic <dej...@gm...> Date: Wed Mar 23 15:23:51 2011 +0100 snap map compiles with eigen commit 7094fbd46755c501c9f62e7d16e6c794a43b807f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 18:05:25 2011 +0100 moved 3d perception to new class commit 861fcc248bd1a1013d95289b6ddafe34b3da787e Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 15:22:05 2011 +0100 moved SnapMapICP out of ias_drawer_executive git-commit Modified Paths: -------------- highlevel/SnapMapICP/manifest.xml highlevel/SnapMapICP/src/SnapMapICP.cpp Modified: highlevel/SnapMapICP/manifest.xml =================================================================== --- highlevel/SnapMapICP/manifest.xml 2012-02-09 10:13:47 UTC (rev 990) +++ highlevel/SnapMapICP/manifest.xml 2012-02-10 13:18:43 UTC (rev 991) @@ -8,6 +8,7 @@ <license>BSD</license> <review status="unreviewed" notes=""/> <url>http://ros.org/wiki/SnapMapICP</url> + <depend package="std_msgs"/> <depend package="pcl"/> <depend package="nav_msgs"/> <depend package="laser_geometry"/> Modified: highlevel/SnapMapICP/src/SnapMapICP.cpp =================================================================== --- highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-02-09 10:13:47 UTC (rev 990) +++ highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-02-10 13:18:43 UTC (rev 991) @@ -29,6 +29,7 @@ #include <ros/ros.h> +#include <std_msgs/String.h> #include <sensor_msgs/PointCloud.h> #include <sensor_msgs/PointCloud2.h> #include <nav_msgs/OccupancyGrid.h> @@ -44,22 +45,40 @@ #include <pcl/registration/icp.h> #include <pcl/registration/icp_nl.h> #include <pcl/registration/transforms.h> +#include <pcl/kdtree/kdtree_flann.h> #include <geometry_msgs/PoseWithCovarianceStamped.h> +#include <boost/thread/mutex.hpp> + +boost::mutex scan_callback_mutex; + //these should be parameters // defines how good the match has to be to create a candidate for publishing a pose -float ICP_FITNESS_THRESHOLD = 100.1;// = 0.025; +double ICP_FITNESS_THRESHOLD = 100.1;// = 0.025; //defines how much distance deviation from amcl to icp pose is needed to make us publish a pose -float DIST_THRESHOLD = 0.05; +double DIST_THRESHOLD = 0.05; // same for angle -float ANGLE_THRESHOLD = 0.01; +double ANGLE_THRESHOLD = 0.01; +double ANGLE_UPPER_THRESHOLD = M_PI / 6; +// accept only scans one second old or younger +double AGE_THRESHOLD = 1; +double UPDATE_AGE_THRESHOLD = 1; +double ICP_INLIER_THRESHOLD = 0.9; +double ICP_INLIER_DIST = 0.1; +double POSE_COVARIANCE_TRANS = 1.5; +double ICP_NUM_ITER = 250; + +std::string BASE_LASER_FRAME = "/base_laser_link"; +std::string ODOM_FRAME = "/odom_combined"; + ros::NodeHandle *nh = 0; ros::Publisher pub_output_; ros::Publisher pub_output_scan; ros::Publisher pub_output_scan_transformed; +ros::Publisher pub_info_; ros::Publisher pub_pose; @@ -78,6 +97,8 @@ bool we_have_a_scan = false; bool we_have_a_scan_transformed = false; +bool use_sim_time = false; + int lastScan = 0; int actScan = 0; @@ -124,6 +145,21 @@ } +boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_xyz; + +pcl::KdTree<pcl::PointXYZ>::Ptr mapTree; + + +pcl::KdTree<pcl::PointXYZ>::Ptr getTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloudb) +{ + pcl::KdTree<pcl::PointXYZ>::Ptr tree; + tree.reset (new pcl::KdTreeFLANN<pcl::PointXYZ>); + + tree->setInputCloud (cloudb); + return tree; +} + + void mapCallback(const nav_msgs::OccupancyGrid& msg) { ROS_DEBUG("I heard frame_id: [%s]", msg.header.frame_id.c_str()); @@ -135,7 +171,7 @@ float posx = msg.info.origin.position.x; float posy = msg.info.origin.position.y; - boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_xyz = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZ> >(new pcl::PointCloud<pcl::PointXYZ>()); + cloud_xyz = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZ> >(new pcl::PointCloud<pcl::PointXYZ>()); //cloud_xyz->width = 100; // 100 cloud_xyz->height = 1; @@ -160,6 +196,8 @@ } cloud_xyz->width = cloud_xyz->points.size(); + mapTree = getTree(cloud_xyz); + pcl::toROSMsg (*cloud_xyz, *output_cloud); ROS_DEBUG("Publishing PointXYZ cloud with %ld points in frame %s", cloud_xyz->points.size(),output_cloud->header.frame_id.c_str()); @@ -171,16 +209,76 @@ int count_sc_ = 0; +bool getTransform(tf::StampedTransform &trans , const std::string parent_frame, const std::string child_frame, const ros::Time stamp) +{ + bool gotTransform = false; + + if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.05))) + { + ROS_ERROR("DIDNT GET TRANSFORM %s %s IN c at %f", parent_frame.c_str(), child_frame.c_str(), stamp.toSec()); + return false; + } + + try + { + gotTransform = true; + listener_->lookupTransform(parent_frame,child_frame,stamp , trans); + } + catch (tf::TransformException ex) + { + gotTransform = false; + ROS_ERROR("DIDNT GET TRANSFORM %s %s IN B", parent_frame.c_str(), child_frame.c_str()); + } + + + return gotTransform; +} + + void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in) { + if (!we_have_a_map) + { + ROS_INFO("Waiting for map"); + return; + } //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_); + if (!scan_callback_mutex.try_lock()) + return; + count_sc_++; - if (count_sc_ > 10) + //ROS_INFO("count_sc %i MUTEX LOCKED", count_sc_); + + //if (count_sc_ > 10) + //if (count_sc_ > 10) { count_sc_ = 0; + ros::Time scan_in_time = scan_in->header.stamp; + tf::StampedTransform base_at_laser; + if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time)) + { + ROS_ERROR("Did not get base pose at laser scan time"); + scan_callback_mutex.unlock(); + + return; + } + + // nh.param<std::string>("default_param", default_param, "default_value"); + nh->param<bool>("USE_SIM_TIME", use_sim_time, false); + nh->param<double>("SnapMapICP/icp_fitness_threshold", ICP_FITNESS_THRESHOLD, 100 ); + nh->param<double>("SnapMapICP/age_threshold", AGE_THRESHOLD, 1); + nh->param<double>("SnapMapICP/angle_upper_threshold", ANGLE_UPPER_THRESHOLD, 1); + nh->param<double>("SnapMapICP/angle_threshold", ANGLE_THRESHOLD, 0.01); + nh->param<double>("SnapMapICP/update_age_threshold", UPDATE_AGE_THRESHOLD, 1); + nh->param<double>("SnapMapICP/dist_threshold", DIST_THRESHOLD, 0.01); + nh->param<double>("SnapMapICP/icp_inlier_threshold", ICP_INLIER_THRESHOLD, 0.88); + nh->param<double>("SnapMapICP/icp_inlier_dist", ICP_INLIER_DIST, 0.1); + nh->param<double>("SnapMapICP/icp_num_iter", ICP_NUM_ITER, 250); + nh->param<double>("SnapMapICP/pose_covariance_trans", POSE_COVARIANCE_TRANS, 0.5); + sensor_msgs::PointCloud cloud; sensor_msgs::PointCloud cloudInMap; @@ -188,6 +286,22 @@ we_have_a_scan = false; bool gotTransform = false; + + if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05))) + { + scan_callback_mutex.unlock(); + ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED"); + return; + } + + if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05))) + { + scan_callback_mutex.unlock(); + ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED"); + return; + } + + while (!gotTransform && (ros::ok())) { try @@ -198,9 +312,16 @@ catch (...) { gotTransform = false; + ROS_ERROR("DIDNT GET TRANSFORM IN A"); } } + for (size_t k =0; k < cloudInMap.points.size(); k++) + { + cloudInMap.points[k].z = 0; + } + + gotTransform = false; tf::StampedTransform oldPose; while (!gotTransform && (ros::ok())) @@ -208,12 +329,13 @@ try { gotTransform = true; - listener_->lookupTransform("/map", "/base_footprint", - ros::Time(0), oldPose); + listener_->lookupTransform("/map", "/base_link", + cloud.header.stamp , oldPose); } catch (tf::TransformException ex) { gotTransform = false; + ROS_ERROR("DIDNT GET TRANSFORM IN B"); } } if (we_have_a_map && gotTransform) @@ -228,68 +350,133 @@ reg.setTransformationEpsilon (1e-6); // Set the maximum distance between two correspondences (src<->tgt) to 10cm // Note: adjust this based on the size of your datasets - reg.setMaxCorrespondenceDistance(0.5); - reg.setMaximumIterations (100); + reg.setMaxCorrespondenceDistance(1.5); + reg.setMaximumIterations (ICP_NUM_ITER); // Set the point representation - PointCloudT myMapCloud; - PointCloudT myScanCloud; + PointCloudT::Ptr myMapCloud (new PointCloudT()); + PointCloudT::Ptr myScanCloud (new PointCloudT()); - pcl::fromROSMsg(*output_cloud,myMapCloud); - pcl::fromROSMsg(cloud2,myScanCloud); + pcl::fromROSMsg(*output_cloud,*myMapCloud); + pcl::fromROSMsg(cloud2,*myScanCloud); - reg.setInputCloud(boost::make_shared< const PointCloudT > (myScanCloud)); - reg.setInputTarget(boost::make_shared< const PointCloudT > (myMapCloud)); + reg.setInputCloud(myScanCloud); + reg.setInputTarget(myMapCloud); PointCloudT unused; int i = 0; reg.align (unused); //-- running the registration in a loop to visualize intermediate results - Eigen::Matrix4f transf = reg.getFinalTransformation(); + const Eigen::Matrix4f &transf = reg.getFinalTransformation(); tf::Transform t; matrixAsTransfrom(transf,t); we_have_a_scan_transformed = false; PointCloudT transformedCloud; - pcl::transformPointCloud (myScanCloud, transformedCloud, reg.getFinalTransformation()); + pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation()); + + double inlier_perc = 0; + + { + // count inliers + + std::vector<int> nn_indices (1); + std::vector<float> nn_sqr_dists (1); + + size_t numinliers = 0; + + for (size_t k = 0; k < transformedCloud.points.size(); ++k ) + { + if (mapTree->radiusSearch (transformedCloud.points[k], ICP_INLIER_DIST, nn_indices,nn_sqr_dists, 1) != 0) + numinliers += 1; + } + if (transformedCloud.points.size() > 0) + { + ROS_WARN("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD); + inlier_perc = (double) numinliers / (double) transformedCloud.points.size(); + } + } + pcl::toROSMsg (transformedCloud, cloud2transformed); we_have_a_scan_transformed = true; - double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y())); double angleDist = t.getRotation().getAngle(); - t = t * oldPose ; + btVector3 rotAxis = t.getRotation().getAxis(); + t = t * oldPose; + tf::StampedTransform base_after_icp; + if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0))) + { + ROS_ERROR("Did not get base pose at now"); + scan_callback_mutex.unlock(); + + return; + } + else + { + tf::Transform rel = base_at_laser.inverseTimes(base_after_icp); + ROS_INFO("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI); + t= t * rel; + } + //ROS_INFO("dist %f angleDist %f",dist, angleDist); + ros::Time scan_time = scan_in->header.stamp; + ros::Duration scan_age = ros::Time::now() - scan_time; + //ROS_INFO("SCAN_AGE seems to be %f", scan_age.toSec()); + char msg_c_str[2048]; + sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD ); + std_msgs::String strmsg; + strmsg.data = msg_c_str; - if ((actScan - lastTimeSent > 10) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD))) - if ( reg.getFitnessScore() <= ICP_FITNESS_THRESHOLD ) + //ROS_INFO("%s", msg_c_str); + + + //check if we want to accept this scan, if its older than 1 sec, drop it + if (!use_sim_time) + if (scan_age.toSec() > AGE_THRESHOLD) { - lastTimeSent = actScan; - geometry_msgs::PoseWithCovarianceStamped pose; - pose.header.frame_id = "map"; - pose.pose.pose.position.x = t.getOrigin().x(); - pose.pose.pose.position.y = t.getOrigin().y(); + ROS_ERROR("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); + scan_callback_mutex.unlock(); - btQuaternion quat = t.getRotation(); - //quat.setRPY(0.0, 0.0, theta); - tf::quaternionTFToMsg(quat,pose.pose.pose.orientation); - float factorPos = 0.03; - float factorRot = 0.1; - pose.pose.covariance[6*0+0] = (0.5 * 0.5) * factorPos; - pose.pose.covariance[6*1+1] = (0.5 * 0.5) * factorPos; - pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot; - ROS_INFO("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() ); - ROS_INFO("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str()); - pub_pose.publish(pose); + return; } + + + double cov = POSE_COVARIANCE_TRANS; + + //if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD)) + // if ( reg.getFitnessScore() <= ICP_FITNESS_THRESHOLD ) + if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (inlier_perc > ICP_INLIER_THRESHOLD) && (angleDist < ANGLE_UPPER_THRESHOLD)) + { + lastTimeSent = actScan; + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.pose.pose.position.x = t.getOrigin().x(); + pose.pose.pose.position.y = t.getOrigin().y(); + + btQuaternion quat = t.getRotation(); + //quat.setRPY(0.0, 0.0, theta); + tf::quaternionTFToMsg(quat,pose.pose.pose.orientation); + float factorPos = 0.03; + float factorRot = 0.1; + pose.pose.covariance[6*0+0] = (cov * cov) * factorPos; + pose.pose.covariance[6*1+1] = (cov * cov) * factorPos; + pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot; + ROS_INFO("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() ); + ROS_INFO("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str()); + pub_pose.publish(pose); + strmsg.data += " << SENT"; + } + + pub_info_.publish(strmsg); //ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str()); //ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str()); } } - // Do something with cloud. + scan_callback_mutex.unlock(); } int main(int argc, char** argv) @@ -300,10 +487,15 @@ ros::NodeHandle nh_; nh = &nh_; + nh->param<std::string>("SnapMapICP/odom_frame", ODOM_FRAME, "/odom_combined"); + nh->param<std::string>("SnapMapICP/base_laser_frame", BASE_LASER_FRAME, "/base_laser_link"); + + projector_ = new laser_geometry::LaserProjection(); tf::TransformListener listener; listener_ = &listener; + pub_info_ = nh->advertise<std_msgs::String> ("SnapMapICP", 1); pub_output_ = nh->advertise<sensor_msgs::PointCloud2> ("map_points", 1); pub_output_scan = nh->advertise<sensor_msgs::PointCloud2> ("scan_points", 1); pub_output_scan_transformed = nh->advertise<sensor_msgs::PointCloud2> ("scan_points_transformed", 1); @@ -317,7 +509,7 @@ listener_->waitForTransform("/base_link", "/map", ros::Time(0), ros::Duration(30.0)); - listener_->waitForTransform("/base_laser_link", "/map", + listener_->waitForTransform(BASE_LASER_FRAME, "/map", ros::Time(0), ros::Duration(30.0)); while (ros::ok()) @@ -340,101 +532,3 @@ } } - - -/* -Full name: /Motors/EtherCAT Device (r_wrist_l_motor) -Component: EtherCAT Device (r_wrist_l_motor) -Hardware ID: 68-05005-01769 -Level: Warning -Message: Could not collect WG0X diagnostics; Could not collect diagnostics - -Configuration: good -Name: r_wrist_l_motor -Position: 31 -Product code: WG05 (6805005) Firmware Revision 1.21, PCB Revision F.02 -Robot: PR2 -Motor: Maxon 310009 -Serial Number: 68-05005-01769 -Nominal Current Scale: 0.000500 -Nominal Voltage Scale: 0.006250 -HW Max Current: 3.250000 -SW Max Current: 1.720000 -Speed Constant: 178.000000 -Resistance: 2.520000 -Motor Torque Constant: 0.053800 -Pulses Per Revolution: 1200 -Encoder Reduction: -1.000000 -Status Checksum Error Count: 0 -Safety Disable Status: ENABLED (00) -Safety Disable Status Hold: ENABLED (00) -Safety Disable Count: 6 -Undervoltage Count: 5 -Over Current Count: 0 -Board Over Temp Count: 0 -Bridge Over Temp Count: 0 -Operate Disable Count: 1 -Watchdog Disable Count: 1 -MBX Command IRQ Count: 181 -PDI Timeout Error Count: 0 -PDI Checksum Error Count: 0 -Supply Current: 0.051398 -Configured Offset A: 0.022000 -Configured Offset B: -0.006000 -Mailbox Write Errors: 0 -Mailbox Read Errors: 0 -Mailbox Retries: 0 -Mailbox Retry Errors: 0 -Calibration Offset: -198.797365 -Calibration Status: Calibrated by controller -Watchdog Limit: 100ms -Mode: ENABLE, CURRENT -Digital out: 254 -Programmed pwm value: -291 -Programmed current: -0.197000 -Measured current: -0.194500 -Timestamp: 3546451304 -Encoder count: -38034 -Encoder index pos: -38034 -Num encoder_errors: 0 -Encoder status: 15 -Calibration reading: 15 -Last calibration rising edge: -43040 -Last calibration falling edge: -18976 -Board temperature: 44.250000 -Max board temperature: 44.375000 -Bridge temperature: 50.687500 -Max bridge temperature: 50.812500 -Supply voltage: 70.787498 -Motor voltage: -1.125000 -Current Loop Kp: 16 -Current Loop Ki: 4 -Motor Voltage Error %: 10.172653 -Max Motor Voltage Error %: 34.376442 -Abs Filtered Voltage Error %: 8.273659 -Max Abs Filtered Voltage Error %: 15.651860 -Current Error: 0.000720 -Max Current Error: 0.017975 -Abs Filtered Current Error: 0.000570 -Max Abs Filtered Current Error: 0.003362 -Motor Resistance Estimate: 4.495118 -# Published traces: 5 -Packet count: 48769 -Drops: 0 -Consecutive Drops: 0 -Max Consecutive Drops: 0 -Reset detected: No -Valid: No -EPU Errors: 0 -PDI Errors: 0 -Status Port 0: Has Link, Open, Has Comm -RX Error Port 0: 0 -Forwarded RX Error Port 0: 0 -Invalid Frame Port 0: 0 -Lost Link Port 0: 0 -Status Port 1: Has Link, Open, Has Comm -RX Error Port 1: 0 -Forwarded RX Error Port 1: 0 -Invalid Frame Port 1: 0 -Lost Link Port 1: 0 -*/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |