├── .gitignore
├── README.md
├── src
    ├── TFpub.cpp
    ├── FreezeCloud2.cpp
    └── SnapMapICP.cpp
├── CMakeLists.txt
├── package.xml
└── launch
    └── snap_map_icp.launch


/.gitignore:
--------------------------------------------------------------------------------
1 | bin
2 | lib
3 | SVN_EXPORT
4 | *~
5 | build
6 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | snap_map_icp
 2 | ----------
 3 | 
 4 | snap_map_icp provides ROS node relocalizing a ROS-enabled robot based on it's base laser readings and matching them with the currently advertised /map topic.
 5 | 
 6 | See:
 7 | http://ros.org/wiki/SnapMapICP
 8 | 
 9 | This is a fork of the discontinued code at:
10 | https://svn.code.sf.net/p/tum-ros-pkg/code/highlevel/SnapMapICP/
11 | 


--------------------------------------------------------------------------------
/src/TFpub.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | 
 3 | #include <ros/ros.h>
 4 | #include <tf/transform_broadcaster.h>
 5 | 
 6 | std::string turtle_name;
 7 | 
 8 | 
 9 | 
10 | void poseCallback(){
11 |   static tf::TransformBroadcaster br;
12 |   tf::Transform transform;
13 |   transform.setOrigin( tf::Vector3(0, 0, 0.0) );
14 |   transform.setRotation( tf::Quaternion(0, 0, 0) );
15 |   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/odom_combined", "/base_footprint"));
16 | }
17 | 
18 | int main(int argc, char** argv){
19 |   ros::init(argc, argv, "tf_pub_broadcaster");
20 | 
21 |   ros::NodeHandle node;
22 |   //ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
23 |   ros::Rate rt(10);
24 |   while (ros::ok()) {
25 |     rt.sleep();
26 |     poseCallback();
27 |     ros::spinOnce();
28 |   }
29 | 
30 |   //  ros::spin();
31 |   return 0;
32 | };
33 | 
34 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(snap_map_icp)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 | 	roscpp
 6 | 	laser_geometry
 7 | 	pcl_ros
 8 |         pcl_conversions
 9 | 	tf
10 |         std_msgs
11 |         nav_msgs
12 | 	cmake_modules)
13 | 
14 | find_package(Eigen REQUIRED)
15 | find_package(PCL REQUIRED)
16 | 
17 | add_definitions(${EIGEN_DEFINITIONS})
18 | 
19 | include_directories(
20 |   ${catkin_INCLUDE_DIRS}
21 |   ${EIGEN_INCLUDE_DIRS}
22 |   ${PCL_INCLUDE_DIRS})
23 | 
24 | catkin_package()
25 | 
26 | add_executable(snap_map_icp src/SnapMapICP.cpp)
27 | add_executable(tf_pub src/TFpub.cpp)
28 | add_executable(freeze_cloud2 src/FreezeCloud2.cpp)
29 | 
30 | target_link_libraries(tf_pub ${catkin_LIBRARIES})
31 | target_link_libraries(freeze_cloud2 ${catkin_LIBRARIES})
32 | target_link_libraries(snap_map_icp ${catkin_LIBRARIES} ${PCL_LIBRARIES})
33 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>snap_map_icp</name>
 3 |   <description>
 4 |     snap_map_icp provides a ROS node relocalizing a ROS-enabled robot based on
 5 |     it's base laser readings and matching them with the currently advertised
 6 |     /map topic.
 7 |   </description>
 8 |   <author>Thomas Ruehr</author>
 9 |   <license>BSD</license>
10 |   <version>0.0.0</version>
11 |   <maintainer email="mhorst@cs.uni-bremen.de">Moritz Horstmann</maintainer>
12 |   <url>http://ros.org/wiki/SnapMapICP</url>
13 | 
14 |   <buildtool_depend>catkin</buildtool_depend>
15 |   <build_depend>std_msgs</build_depend>
16 |   <build_depend>roscpp</build_depend>
17 |   <build_depend>libpcl-all</build_depend>
18 |   <build_depend>pcl_ros</build_depend>
19 |   <build_depend>pcl_conversions</build_depend>
20 |   <build_depend>nav_msgs</build_depend>
21 |   <build_depend>laser_geometry</build_depend>
22 |   <build_depend>tf</build_depend>
23 |   <build_depend>cmake_modules</build_depend>
24 |   <run_depend>std_msgs</run_depend>
25 |   <run_depend>roscpp</run_depend>
26 |   <run_depend>libpcl-all</run_depend>
27 |   <run_depend>pcl_ros</run_depend>
28 |   <run_depend>pcl_conversions</run_depend>
29 |   <run_depend>nav_msgs</run_depend>
30 |   <run_depend>laser_geometry</run_depend>
31 |   <run_depend>tf</run_depend>
32 | </package>
33 | 


--------------------------------------------------------------------------------
/launch/snap_map_icp.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <node pkg="snap_map_icp" type="snap_map_icp" name="snap_map_icp" output="screen">
 3 |     <!-- relevant scan frames -->
 4 |     <param name="odom_frame" value="/odom_combined"/>
 5 |     <param name="base_laser_frame" value="/base_laser_link"/>
 6 | 
 7 |     <!-- defines minimum fitness for candidates for pose publishing -->
 8 |     <param name="icp_fitness_threshold" value="100"/>
 9 |  
10 |     <!-- accept only scans one second old or younger -->
11 |     <param name="age_threshold" value="1"/>
12 |     <param name="update_age_threshold" value="1"/>
13 | 
14 |     <!-- thresholds over distance btw amcl icp pose to trigger pose publishing -->
15 |     <!-- translation -->
16 |     <param name="dist_threshold" value="0.01"/>
17 |     <!-- rotation -->
18 |     <param name="angle_threshold" value="0.01"/>
19 |     <param name="angle_upper_threshold" value="1"/>
20 | 
21 |     <!-- various icp parameters -->
22 |     <!-- consult with icp algorithm from pcl for details -->
23 |     <param name="icp_inlier_threshold" value="0.88"/>
24 |     <param name="icp_inlier_dist" value="0.1"/>
25 |     <param name="icp_num_iter" value="250"/>
26 | 
27 |     <!-- who knows??? -->
28 |     <param name="pose_covariance_trans" value="0.5"/>
29 | 
30 |     <!-- rate at which scans are taken -->
31 |     <param name="scan_rate" value="2"/>
32 |   </node>
33 | </launch>
34 | 


--------------------------------------------------------------------------------
/src/FreezeCloud2.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
 3 |  * All rights reserved.
 4 |  *
 5 |  * Redistribution and use in source and binary forms, with or without
 6 |  * modification, are permitted provided that the following conditions are met:
 7 |  *
 8 |  *     * Redistributions of source code must retain the above copyright
 9 |  *       notice, this list of conditions and the following disclaimer.
10 |  *     * Redistributions in binary form must reproduce the above copyright
11 |  *       notice, this list of conditions and the following disclaimer in the
12 |  *       documentation and/or other materials provided with the distribution.
13 |  *     * Neither the name of Willow Garage, Inc. nor the names of its
14 |  *       contributors may be used to endorse or promote products derived from
15 |  *       this software without specific prior written permission.
16 |  *
17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 |  * POSSIBILITY OF SUCH DAMAGE.
28 |  */
29 | 
30 | #include <ros/ros.h>
31 | 
32 | #include <sensor_msgs/PointCloud2.h>
33 | #include <string>
34 | 
35 | int main(int argc, char** argv)
36 | {
37 | 
38 |     // Init the ROS node
39 |     ros::init(argc, argv, "freezecloud2");
40 | 
41 |         ROS_INFO("USAGE: FreezeCloud2 input_topic output_topic");
42 |         ROS_INFO("Will wait for a PointCloud2 on input_topic and publish it once on output_topic");
43 |     if (argc > 2) {
44 |         ROS_INFO("input_topic %s output_topic %s", argv[1], argv[2]);
45 |     } else {
46 |         exit(0);
47 |     }
48 | 
49 |     ros::NodeHandle nh_;
50 | 
51 |     ros::Rate loop_rate(10);
52 | 
53 |     sensor_msgs::PointCloud2 cloud2 = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>(argv[1]));
54 | 
55 |     ros::Publisher pub = nh_.advertise<sensor_msgs::PointCloud2> (std::string(argv[2]),1,true);
56 | 
57 |     pub.publish(cloud2);
58 |     ROS_INFO("PUBLISHED");
59 |     while (ros::ok())
60 |     {
61 |         loop_rate.sleep();
62 |         ros::spinOnce();
63 |     }
64 | 
65 | }
66 | 


--------------------------------------------------------------------------------
/src/SnapMapICP.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
  3 |  * All rights reserved.
  4 |  *
  5 |  * Redistribution and use in source and binary forms, with or without
  6 |  * modification, are permitted provided that the following conditions are met:
  7 |  *
  8 |  *     * Redistributions of source code must retain the above copyright
  9 |  *       notice, this list of conditions and the following disclaimer.
 10 |  *     * Redistributions in binary form must reproduce the above copyright
 11 |  *       notice, this list of conditions and the following disclaimer in the
 12 |  *       documentation and/or other materials provided with the distribution.
 13 |  *     * Neither the name of Willow Garage, Inc. nor the names of its
 14 |  *       contributors may be used to endorse or promote products derived from
 15 |  *       this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 |  * POSSIBILITY OF SUCH DAMAGE.
 28 |  */
 29 | 
 30 | #include <ros/ros.h>
 31 | 
 32 | #include <std_msgs/String.h>
 33 | #include <sensor_msgs/PointCloud.h>
 34 | #include <sensor_msgs/PointCloud2.h>
 35 | #include <nav_msgs/OccupancyGrid.h>
 36 | #include <pcl_conversions/pcl_conversions.h>
 37 | #include <sensor_msgs/point_cloud_conversion.h>
 38 | #include <sensor_msgs/LaserScan.h>
 39 | #include <pcl/point_types.h>
 40 | #include <pcl/io/pcd_io.h>
 41 | #include <laser_geometry/laser_geometry.h>
 42 | #include <tf/transform_listener.h>
 43 | 
 44 | #include <pcl/registration/icp.h>
 45 | #include <pcl/registration/icp_nl.h>
 46 | #include <pcl/registration/transforms.h>
 47 | #include <pcl/kdtree/kdtree_flann.h>
 48 | 
 49 | #include <geometry_msgs/PoseWithCovarianceStamped.h>
 50 | 
 51 | #include <boost/thread/mutex.hpp>
 52 | 
 53 | boost::mutex scan_callback_mutex;
 54 | 
 55 | //these should be parameters
 56 | // defines how good the match has to be to create a candidate for publishing a pose
 57 | double ICP_FITNESS_THRESHOLD = 100.1;// =  0.025;
 58 | //defines how much distance deviation from amcl to icp pose is needed to make us publish a pose
 59 | double DIST_THRESHOLD = 0.05;
 60 | // same for angle
 61 | double ANGLE_THRESHOLD = 0.01;
 62 | double ANGLE_UPPER_THRESHOLD = M_PI / 6;
 63 | // accept only scans one second old or younger
 64 | double AGE_THRESHOLD = 1;
 65 | double UPDATE_AGE_THRESHOLD = 1;
 66 | 
 67 | double ICP_INLIER_THRESHOLD = 0.9;
 68 | double ICP_INLIER_DIST = 0.1;
 69 | 
 70 | double POSE_COVARIANCE_TRANS = 1.5;
 71 | double ICP_NUM_ITER = 250;
 72 | 
 73 | double SCAN_RATE = 2;
 74 | 
 75 | std::string BASE_LASER_FRAME = "/base_laser_link";
 76 | std::string ODOM_FRAME = "/odom_combined";
 77 | 
 78 | ros::NodeHandle *nh = 0;
 79 | ros::Publisher pub_output_;
 80 | ros::Publisher pub_output_scan;
 81 | ros::Publisher pub_output_scan_transformed;
 82 | ros::Publisher pub_info_;
 83 | 
 84 | ros::Publisher pub_pose;
 85 | 
 86 | 
 87 | laser_geometry::LaserProjection *projector_ = 0;
 88 | tf::TransformListener *listener_ = 0;
 89 | sensor_msgs::PointCloud2 cloud2;
 90 | sensor_msgs::PointCloud2 cloud2transformed;
 91 | 
 92 | typedef pcl::PointCloud<pcl::PointXYZ> PointCloudT;
 93 | 
 94 | boost::shared_ptr< sensor_msgs::PointCloud2> output_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2());
 95 | boost::shared_ptr< sensor_msgs::PointCloud2> scan_cloud = boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2());
 96 | 
 97 | bool we_have_a_map = false;
 98 | bool we_have_a_scan = false;
 99 | bool we_have_a_scan_transformed = false;
100 | 
101 | bool use_sim_time = false;
102 | 
103 | int lastScan = 0;
104 | int actScan = 0;
105 | 
106 | /*inline void
107 |   pcl::transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
108 | {
109 |   double mv[12];
110 |   bt.getBasis ().getOpenGLSubMatrix (mv);
111 | 
112 |   tf::Vector3 origin = bt.getOrigin ();
113 | 
114 |   out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
115 |   out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
116 |   out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
117 | 
118 |   out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
119 |   out_mat (0, 3) = origin.x ();
120 |   out_mat (1, 3) = origin.y ();
121 |   out_mat (2, 3) = origin.z ();
122 | }*/
123 | 
124 | inline void
125 | matrixAsTransfrom (const Eigen::Matrix4f &out_mat,  tf::Transform& bt)
126 | {
127 |     double mv[12];
128 | 
129 |     mv[0] = out_mat (0, 0) ;
130 |     mv[4] = out_mat (0, 1);
131 |     mv[8] = out_mat (0, 2);
132 |     mv[1] = out_mat (1, 0) ;
133 |     mv[5] = out_mat (1, 1);
134 |     mv[9] = out_mat (1, 2);
135 |     mv[2] = out_mat (2, 0) ;
136 |     mv[6] = out_mat (2, 1);
137 |     mv[10] = out_mat (2, 2);
138 | 
139 |     tf::Matrix3x3 basis;
140 |     basis.setFromOpenGLSubMatrix(mv);
141 |     tf::Vector3 origin(out_mat (0, 3),out_mat (1, 3),out_mat (2, 3));
142 | 
143 |     ROS_DEBUG("origin %f %f %f", origin.x(), origin.y(), origin.z());
144 | 
145 |     bt = tf::Transform(basis,origin);
146 | }
147 | 
148 | 
149 | boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_xyz;
150 | 
151 | pcl::KdTree<pcl::PointXYZ>::Ptr mapTree;
152 | 
153 | 
154 | pcl::KdTree<pcl::PointXYZ>::Ptr getTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloudb)
155 | {
156 |     pcl::KdTree<pcl::PointXYZ>::Ptr tree;
157 |     tree.reset (new pcl::KdTreeFLANN<pcl::PointXYZ>);
158 | 
159 |     tree->setInputCloud (cloudb);
160 |     return tree;
161 | }
162 | 
163 | 
164 | void mapCallback(const nav_msgs::OccupancyGrid& msg)
165 | {
166 |     ROS_DEBUG("I heard frame_id: [%s]", msg.header.frame_id.c_str());
167 | 
168 |     float resolution = msg.info.resolution;
169 |     float width = msg.info.width;
170 |     float height = msg.info.height;
171 | 
172 |     float posx = msg.info.origin.position.x;
173 |     float posy = msg.info.origin.position.y;
174 | 
175 |     cloud_xyz = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZ> >(new pcl::PointCloud<pcl::PointXYZ>());
176 | 
177 |     //cloud_xyz->width    = 100; // 100
178 |     cloud_xyz->height   = 1;
179 |     cloud_xyz->is_dense = false;
180 |     std_msgs::Header header;
181 |     header.stamp = ros::Time(0);
182 |     header.frame_id = "/map";
183 |     cloud_xyz->header = pcl_conversions::toPCL(header);
184 | 
185 |     pcl::PointXYZ point_xyz;
186 | 
187 |     //for (unsigned int i = 0; i < cloud_xyz->width ; i++)
188 |     for (int y = 0; y < height; y++)
189 |         for (int x = 0; x < width; x++)
190 |         {
191 |             //@TODO
192 |             if (msg.data[x + y * width] == 100)
193 |             {
194 |                 point_xyz.x = (.5f + x) * resolution + posx;
195 |                 point_xyz.y = (.5f + y) * resolution + posy;
196 |                 point_xyz.z = 0;
197 |                 cloud_xyz->points.push_back(point_xyz);
198 |             }
199 |         }
200 |     cloud_xyz->width = cloud_xyz->points.size();
201 | 
202 |     mapTree = getTree(cloud_xyz);
203 | 
204 |     pcl::toROSMsg (*cloud_xyz, *output_cloud);
205 |     ROS_DEBUG("Publishing PointXYZ cloud with %ld points in frame %s", cloud_xyz->points.size(),output_cloud->header.frame_id.c_str());
206 | 
207 |     we_have_a_map = true;
208 | }
209 | 
210 | 
211 | int lastTimeSent = -1000;
212 | 
213 | int count_sc_ = 0;
214 | 
215 | bool getTransform(tf::StampedTransform &trans , const std::string parent_frame, const std::string child_frame, const ros::Time stamp)
216 | {
217 |     bool gotTransform = false;
218 | 
219 |     ros::Time before = ros::Time::now();
220 |     if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.5)))
221 |     {
222 |         ROS_WARN("DIDNT GET TRANSFORM %s %s IN c at %f", parent_frame.c_str(), child_frame.c_str(), stamp.toSec());
223 |         return false;
224 |     }
225 |     //ROS_INFO("waited for transform %f", (ros::Time::now() - before).toSec());
226 | 
227 |     try
228 |     {
229 |         gotTransform = true;
230 |         listener_->lookupTransform(parent_frame,child_frame,stamp , trans);
231 |     }
232 |     catch (tf::TransformException ex)
233 |     {
234 |         gotTransform = false;
235 |         ROS_WARN("DIDNT GET TRANSFORM %s %s IN B", parent_frame.c_str(), child_frame.c_str());
236 |     }
237 | 
238 | 
239 |     return gotTransform;
240 | }
241 | 
242 | ros::Time last_processed_scan;
243 | 
244 | void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
245 | {
246 |     if (!we_have_a_map)
247 |     {
248 |         ROS_INFO("SnapMapICP waiting for map to be published");
249 |         return;
250 |     }
251 | 
252 |     ros::Time scan_in_time = scan_in->header.stamp;
253 |     ros::Time time_received = ros::Time::now();
254 | 
255 |     if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) )
256 |     {
257 |         ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec());
258 |         return;
259 |     }
260 | 
261 | 
262 |     //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_);
263 |     if (!scan_callback_mutex.try_lock())
264 |         return;
265 | 
266 |     ros::Duration scan_age = ros::Time::now() - scan_in_time;
267 | 
268 |     //check if we want to accept this scan, if its older than 1 sec, drop it
269 |     if (!use_sim_time)
270 |         if (scan_age.toSec() > AGE_THRESHOLD)
271 |         {
272 |             //ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD);
273 |             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() );
274 |             scan_callback_mutex.unlock();
275 | 
276 |             return;
277 |         }
278 | 
279 |     count_sc_++;
280 |     //ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_);
281 | 
282 |     //if (count_sc_ > 10)
283 |     //if (count_sc_ > 10)
284 |     {
285 |         count_sc_ = 0;
286 | 
287 |         tf::StampedTransform base_at_laser;
288 |         if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time))
289 |         {
290 |             ROS_WARN("Did not get base pose at laser scan time");
291 |             scan_callback_mutex.unlock();
292 | 
293 |             return;
294 |         }
295 | 
296 | 
297 |         sensor_msgs::PointCloud cloud;
298 |         sensor_msgs::PointCloud cloudInMap;
299 | 
300 |         projector_->projectLaser(*scan_in,cloud);
301 | 
302 |         we_have_a_scan = false;
303 |         bool gotTransform = false;
304 | 
305 |         if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05)))
306 |         {
307 |             scan_callback_mutex.unlock();
308 |             ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED");
309 |             return;
310 |         }
311 | 
312 |         if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05)))
313 |         {
314 |             scan_callback_mutex.unlock();
315 |             ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED");
316 |             return;
317 |         }
318 | 
319 | 
320 |         while (!gotTransform && (ros::ok()))
321 |         {
322 |             try
323 |             {
324 |                 gotTransform = true;
325 |                 listener_->transformPointCloud ("/map",cloud,cloudInMap);
326 |             }
327 |             catch (...)
328 |             {
329 |                 gotTransform = false;
330 |                 ROS_WARN("DIDNT GET TRANSFORM IN A");
331 |             }
332 |         }
333 | 
334 |         for (size_t k =0; k < cloudInMap.points.size(); k++)
335 |         {
336 |             cloudInMap.points[k].z = 0;
337 |         }
338 | 
339 | 
340 |         gotTransform = false;
341 |         tf::StampedTransform oldPose;
342 |         while (!gotTransform && (ros::ok()))
343 |         {
344 |             try
345 |             {
346 |                 gotTransform = true;
347 |                 listener_->lookupTransform("/map", "/base_link",
348 |                                            cloud.header.stamp , oldPose);
349 |             }
350 |             catch (tf::TransformException ex)
351 |             {
352 |                 gotTransform = false;
353 |                 ROS_WARN("DIDNT GET TRANSFORM IN B");
354 |             }
355 |         }
356 |         if (we_have_a_map && gotTransform)
357 |         {
358 |             sensor_msgs::convertPointCloudToPointCloud2(cloudInMap,cloud2);
359 |             we_have_a_scan = true;
360 | 
361 |             actScan++;
362 | 
363 |             //pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> reg;
364 |             pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
365 |             reg.setTransformationEpsilon (1e-6);
366 |             // Set the maximum distance between two correspondences (src<->tgt) to 10cm
367 |             // Note: adjust this based on the size of your datasets
368 |             reg.setMaxCorrespondenceDistance(0.5);
369 |             reg.setMaximumIterations (ICP_NUM_ITER);
370 |             // Set the point representation
371 | 
372 |             //ros::Time bef = ros::Time::now();
373 | 
374 |             PointCloudT::Ptr myMapCloud (new PointCloudT());
375 |             PointCloudT::Ptr myScanCloud (new PointCloudT());
376 | 
377 |             pcl::fromROSMsg(*output_cloud,*myMapCloud);
378 |             pcl::fromROSMsg(cloud2,*myScanCloud);
379 | 
380 |             reg.setInputCloud(myScanCloud);
381 |             reg.setInputTarget(myMapCloud);
382 | 
383 |             PointCloudT unused;
384 |             int i = 0;
385 | 
386 |             reg.align (unused);
387 | 
388 |             const Eigen::Matrix4f &transf = reg.getFinalTransformation();
389 |             tf::Transform t;
390 |             matrixAsTransfrom(transf,t);
391 | 
392 |             //ROS_ERROR("proc time %f", (ros::Time::now() - bef).toSec());
393 | 
394 |             we_have_a_scan_transformed = false;
395 |             PointCloudT transformedCloud;
396 |             pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation());
397 | 
398 |             double inlier_perc = 0;
399 |             {
400 |                 // count inliers
401 |                 std::vector<int> nn_indices (1);
402 |                 std::vector<float> nn_sqr_dists (1);
403 | 
404 |                 size_t numinliers = 0;
405 | 
406 |                 for (size_t k = 0; k < transformedCloud.points.size(); ++k )
407 |                 {
408 |                     if (mapTree->radiusSearch (transformedCloud.points[k], ICP_INLIER_DIST, nn_indices,nn_sqr_dists, 1) != 0)
409 |                         numinliers += 1;
410 |                 }
411 |                 if (transformedCloud.points.size() > 0)
412 |                 {
413 |                     //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);
414 |                     inlier_perc = (double) numinliers / (double) transformedCloud.points.size();
415 |                 }
416 |             }
417 | 
418 |             last_processed_scan = scan_in_time;
419 | 
420 |             pcl::toROSMsg (transformedCloud, cloud2transformed);
421 |             we_have_a_scan_transformed = true;
422 | 
423 |             double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y()));
424 |             double angleDist = t.getRotation().getAngle();
425 |             tf::Vector3 rotAxis  = t.getRotation().getAxis();
426 |             t =  t * oldPose;
427 | 
428 |             tf::StampedTransform base_after_icp;
429 |             if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0)))
430 |             {
431 |                 ROS_WARN("Did not get base pose at now");
432 |                 scan_callback_mutex.unlock();
433 | 
434 |                 return;
435 |             }
436 |             else
437 |             {
438 |                 tf::Transform rel = base_at_laser.inverseTimes(base_after_icp);
439 |                 ROS_DEBUG("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI);
440 |                 t= t * rel;
441 |             }
442 | 
443 |             //ROS_DEBUG("dist %f angleDist %f",dist, angleDist);
444 | 
445 |             //ROS_DEBUG("SCAN_AGE seems to be %f", scan_age.toSec());
446 |             char msg_c_str[2048];
447 |             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 );
448 |             std_msgs::String strmsg;
449 |             strmsg.data = msg_c_str;
450 | 
451 |             //ROS_DEBUG("%s", msg_c_str);
452 | 
453 |             double cov = POSE_COVARIANCE_TRANS;
454 | 
455 |             //if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD))
456 |             //  if ( reg.getFitnessScore()  <= ICP_FITNESS_THRESHOLD )
457 |             if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (inlier_perc > ICP_INLIER_THRESHOLD) && (angleDist < ANGLE_UPPER_THRESHOLD))
458 |             {
459 |                 lastTimeSent = actScan;
460 |                 geometry_msgs::PoseWithCovarianceStamped pose;
461 |                 pose.header.frame_id = "map";
462 |                 pose.pose.pose.position.x = t.getOrigin().x();
463 |                 pose.pose.pose.position.y = t.getOrigin().y();
464 | 
465 |                 tf::Quaternion quat = t.getRotation();
466 |                 //quat.setRPY(0.0, 0.0, theta);
467 |                 tf::quaternionTFToMsg(quat,pose.pose.pose.orientation);
468 |                 float factorPos = 0.03;
469 |                 float factorRot = 0.1;
470 |                 pose.pose.covariance[6*0+0] = (cov * cov) * factorPos;
471 |                 pose.pose.covariance[6*1+1] = (cov * cov) * factorPos;
472 |                 pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot;
473 |                 ROS_DEBUG("i %i converged %i SCORE: %f", i,  reg.hasConverged (),  reg.getFitnessScore()  );
474 |                 ROS_DEBUG("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());
475 |                 pub_pose.publish(pose);
476 |                 strmsg.data += " << SENT";
477 |             }
478 | 
479 |             //ROS_INFO("processing time : %f", (ros::Time::now() - time_received).toSec());
480 | 
481 |             pub_info_.publish(strmsg);
482 |             //ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str());
483 |             //ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str());
484 |         }
485 |     }
486 |     scan_callback_mutex.unlock();
487 | }
488 | 
489 | 
490 | ros::Time paramsWereUpdated;
491 | 
492 | 
493 | void updateParams()
494 | {
495 |     paramsWereUpdated = ros::Time::now();
496 |     // nh.param<std::string>("default_param", default_param, "default_value");
497 |     nh->param<bool>("/USE_SIM_TIME", use_sim_time, false);
498 |     nh->param<double>("icp_fitness_threshold", ICP_FITNESS_THRESHOLD, 100 );
499 |     nh->param<double>("age_threshold", AGE_THRESHOLD, 1);
500 |     nh->param<double>("angle_upper_threshold", ANGLE_UPPER_THRESHOLD, 1);
501 |     nh->param<double>("angle_threshold", ANGLE_THRESHOLD, 0.01);
502 |     nh->param<double>("update_age_threshold", UPDATE_AGE_THRESHOLD, 1);
503 |     nh->param<double>("dist_threshold", DIST_THRESHOLD, 0.01);
504 |     nh->param<double>("icp_inlier_threshold", ICP_INLIER_THRESHOLD, 0.88);
505 |     nh->param<double>("icp_inlier_dist", ICP_INLIER_DIST, 0.1);
506 |     nh->param<double>("icp_num_iter", ICP_NUM_ITER, 250);
507 |     nh->param<double>("pose_covariance_trans", POSE_COVARIANCE_TRANS, 0.5);
508 |     nh->param<double>("scan_rate", SCAN_RATE, 2);
509 |     if (SCAN_RATE < .001)
510 |         SCAN_RATE  = .001;
511 |     //ROS_DEBUG("PARAM UPDATE");
512 | 
513 | }
514 | 
515 | int main(int argc, char** argv)
516 | {
517 | 
518 | // Init the ROS node
519 |     ros::init(argc, argv, "snapmapicp");
520 |     ros::NodeHandle nh_("~");
521 |     nh = &nh_;
522 | 
523 |     nh->param<std::string>("odom_frame", ODOM_FRAME, "/odom_combined");
524 |     nh->param<std::string>("base_laser_frame", BASE_LASER_FRAME, "/base_laser_link");
525 | 
526 |     last_processed_scan = ros::Time::now();
527 | 
528 |     projector_ = new laser_geometry::LaserProjection();
529 |     tf::TransformListener listener;
530 |     listener_ = &listener;
531 | 
532 |     pub_info_ =  nh->advertise<std_msgs::String> ("SnapMapICP", 1);
533 |     pub_output_ = nh->advertise<sensor_msgs::PointCloud2> ("map_points", 1);
534 |     pub_output_scan = nh->advertise<sensor_msgs::PointCloud2> ("scan_points", 1);
535 |     pub_output_scan_transformed = nh->advertise<sensor_msgs::PointCloud2> ("scan_points_transformed", 1);
536 |     pub_pose = nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
537 | 
538 |     ros::Subscriber subMap = nh_.subscribe("map", 1, mapCallback);
539 |     ros::Subscriber subScan = nh_.subscribe("base_scan", 1, scanCallback);
540 | 
541 |     ros::Rate loop_rate(5);
542 | 
543 |     listener_->waitForTransform("/base_link", "/map",
544 |                                 ros::Time(0), ros::Duration(30.0));
545 | 
546 |     listener_->waitForTransform(BASE_LASER_FRAME, "/map",
547 |                                 ros::Time(0), ros::Duration(30.0));
548 | 
549 |     ros::AsyncSpinner spinner(1);
550 |     spinner.start();
551 | 
552 |     updateParams();
553 | 
554 |     ROS_INFO("SnapMapICP running.");
555 | 
556 | 
557 |     while (ros::ok())
558 |     {
559 |         if (actScan > lastScan)
560 |         {
561 |             lastScan = actScan;
562 |             // publish map as a pointcloud2
563 |             //if (we_have_a_map)
564 |             //   pub_output_.publish(Ŕ);
565 |             // publish scan as seen as a pointcloud2
566 |             //if (we_have_a_scan)
567 |             //   pub_output_scan.publish(cloud2);
568 |             // publish icp transformed scan
569 |             if (we_have_a_scan_transformed)
570 |                 pub_output_scan_transformed.publish(cloud2transformed);
571 |         }
572 |         loop_rate.sleep();
573 |         ros::spinOnce();
574 | 
575 |         if (ros::Time::now() - paramsWereUpdated > ros::Duration(1))
576 |             updateParams();
577 |     }
578 | 
579 | }
580 | 


--------------------------------------------------------------------------------