From: <tum...@li...> - 2012-07-13 11:23:20
|
Revision: 1004 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1004&view=rev Author: moesenle Date: 2012-07-13 11:23:09 +0000 (Fri, 13 Jul 2012) Log Message: ----------- Author: Thomas Ruehr <ru...@in...> Date: Fri Jul 13 13:22:45 2012 +0200 scan_rate parameter. warnings. 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/CMakeLists.txt highlevel/SnapMapICP/src/SnapMapICP.cpp Modified: highlevel/SnapMapICP/CMakeLists.txt =================================================================== --- highlevel/SnapMapICP/CMakeLists.txt 2012-06-21 12:21:05 UTC (rev 1003) +++ highlevel/SnapMapICP/CMakeLists.txt 2012-07-13 11:23:09 UTC (rev 1004) @@ -21,6 +21,10 @@ #uncomment if you have defined services #rosbuild_gensrv() +find_package(Eigen REQUIRED) +include_directories(${EIGEN_INCLUDE_DIRS}) +add_definitions(${EIGEN_DEFINITIONS}) + #common commands for building c++ executables and libraries #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) #target_link_libraries(${PROJECT_NAME} another_library) Modified: highlevel/SnapMapICP/src/SnapMapICP.cpp =================================================================== --- highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-06-21 12:21:05 UTC (rev 1003) +++ highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-07-13 11:23:09 UTC (rev 1004) @@ -71,6 +71,8 @@ double POSE_COVARIANCE_TRANS = 1.5; double ICP_NUM_ITER = 250; +double SCAN_RATE = 2; + std::string BASE_LASER_FRAME = "/base_laser_link"; std::string ODOM_FRAME = "/odom_combined"; @@ -137,7 +139,7 @@ btMatrix3x3 basis; basis.setFromOpenGLSubMatrix(mv); - btVector3 origin(out_mat (0, 3),out_mat (1, 3),out_mat (2, 3)); + tf::Vector3 origin(out_mat (0, 3),out_mat (1, 3),out_mat (2, 3)); ROS_DEBUG("origin %f %f %f", origin.x(), origin.y(), origin.z()); @@ -213,11 +215,13 @@ { bool gotTransform = false; - if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.05))) + ros::Time before = ros::Time::now(); + if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.5))) { ROS_WARN("DIDNT GET TRANSFORM %s %s IN c at %f", parent_frame.c_str(), child_frame.c_str(), stamp.toSec()); return false; } + //ROS_INFO("waited for transform %f", (ros::Time::now() - before).toSec()); try { @@ -234,19 +238,43 @@ return gotTransform; } +ros::Time last_processed_scan; void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in) { - if (!we_have_a_map) { ROS_INFO("SnapMapICP waiting for map to be published"); return; } + + ros::Time scan_in_time = scan_in->header.stamp; + ros::Time time_received = ros::Time::now(); + + if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) ) + { + ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec()); + return; + } + + //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_); if (!scan_callback_mutex.try_lock()) return; + ros::Duration scan_age = ros::Time::now() - scan_in_time; + + //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) + { + //ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); + ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold) scan time: %f , now %f", scan_age.toSec(), AGE_THRESHOLD, scan_in_time.toSec(),ros::Time::now().toSec() ); + scan_callback_mutex.unlock(); + + return; + } + count_sc_++; //ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_); @@ -255,8 +283,6 @@ { 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)) { @@ -266,18 +292,6 @@ 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; @@ -354,6 +368,8 @@ reg.setMaximumIterations (ICP_NUM_ITER); // Set the point representation + //ros::Time bef = ros::Time::now(); + PointCloudT::Ptr myMapCloud (new PointCloudT()); PointCloudT::Ptr myScanCloud (new PointCloudT()); @@ -365,22 +381,22 @@ PointCloudT unused; int i = 0; - reg.align (unused); //-- running the registration in a loop to visualize intermediate results + reg.align (unused); + const Eigen::Matrix4f &transf = reg.getFinalTransformation(); tf::Transform t; matrixAsTransfrom(transf,t); + //ROS_ERROR("proc time %f", (ros::Time::now() - bef).toSec()); + we_have_a_scan_transformed = false; PointCloudT transformedCloud; 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); @@ -393,17 +409,19 @@ } if (transformedCloud.points.size() > 0) { - ROS_DEBUG("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); + //ROS_INFO("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(); } } + last_processed_scan = scan_in_time; + 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(); - btVector3 rotAxis = t.getRotation().getAxis(); + tf::Vector3 rotAxis = t.getRotation().getAxis(); t = t * oldPose; tf::StampedTransform base_after_icp; @@ -423,8 +441,6 @@ //ROS_DEBUG("dist %f angleDist %f",dist, angleDist); - ros::Time scan_time = scan_in->header.stamp; - ros::Duration scan_age = ros::Time::now() - scan_time; //ROS_DEBUG("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 ); @@ -433,18 +449,6 @@ //ROS_DEBUG("%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) - { - ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); - scan_callback_mutex.unlock(); - - return; - } - - double cov = POSE_COVARIANCE_TRANS; //if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD)) @@ -457,7 +461,7 @@ pose.pose.pose.position.x = t.getOrigin().x(); pose.pose.pose.position.y = t.getOrigin().y(); - btQuaternion quat = t.getRotation(); + tf::Quaternion quat = t.getRotation(); //quat.setRPY(0.0, 0.0, theta); tf::quaternionTFToMsg(quat,pose.pose.pose.orientation); float factorPos = 0.03; @@ -471,6 +475,8 @@ strmsg.data += " << SENT"; } + //ROS_INFO("processing time : %f", (ros::Time::now() - time_received).toSec()); + 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()); @@ -479,6 +485,32 @@ scan_callback_mutex.unlock(); } + +ros::Time paramsWereUpdated; + + +void updateParams() +{ + paramsWereUpdated = ros::Time::now(); + // 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); + nh->param<double>("SnapMapICP/scan_rate", SCAN_RATE, 2); + if (SCAN_RATE < .001) + SCAN_RATE = .001; + //ROS_DEBUG("PARAM UPDATE"); + +} + int main(int argc, char** argv) { @@ -490,6 +522,7 @@ 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"); + last_processed_scan = ros::Time::now(); projector_ = new laser_geometry::LaserProjection(); tf::TransformListener listener; @@ -501,10 +534,10 @@ pub_output_scan_transformed = nh->advertise<sensor_msgs::PointCloud2> ("scan_points_transformed", 1); pub_pose = nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1); - ros::Subscriber subMap = nh_.subscribe("map", 10, mapCallback); - ros::Subscriber subScan = nh_.subscribe("base_scan", 10, scanCallback); + ros::Subscriber subMap = nh_.subscribe("map", 1, mapCallback); + ros::Subscriber subScan = nh_.subscribe("base_scan", 1, scanCallback); - ros::Rate loop_rate(10); + ros::Rate loop_rate(5); listener_->waitForTransform("/base_link", "/map", ros::Time(0), ros::Duration(30.0)); @@ -512,6 +545,14 @@ listener_->waitForTransform(BASE_LASER_FRAME, "/map", ros::Time(0), ros::Duration(30.0)); + ros::AsyncSpinner spinner(1); + spinner.start(); + + updateParams(); + + ROS_INFO("SnapMapICP running."); + + while (ros::ok()) { if (actScan > lastScan) @@ -529,6 +570,9 @@ } loop_rate.sleep(); ros::spinOnce(); + + if (ros::Time::now() - paramsWereUpdated > ros::Duration(1)) + updateParams(); } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |