added backup mapping demo

graph_map/launch/3d_fuser_ncfm2018.launch

@@ -22,6 +22,9 @@
       <param name="initPoseFromGT" value="false" />
       <param name="pose_init_x" value="0" />
       <param name="pose_init_y" value="0" />
+      <param name="pose_init_z" value="0" />
+      <param name="pose_init_r" value="0" />
+      <param name="pose_init_p" value="0" />
       <param name="pose_init_t" value="0" />
 
       <param name="save_occupancy" value="true" />
@@ -82,8 +85,6 @@
       <param name="size_z_meters" value="10" />
      
 
-
-
       <!--<param name="laser_variance_z" value="0.02" /> -->
 
       <!--range of sensor, min range can prevent self-mapping of the robot or operator of the lidar, max range can remove uncertain measurments -->

graph_map/launch/visualize.launch

 <?xml version="1.0" encoding="utf-8"?>
 <launch>
-  <arg name="run_bag" default="false"/>
-  <arg name="robot_prefix" default="robot5"/>
-  <arg name="output_pointcloud_topic_name" default="/fused_cloud"/>
-  <arg name="rviz_enabled" default="true"/>
 
-<group if="$(arg rviz_enabled)">
+
   <!-- Include visualization in rviz -->
   <include file="$(find graph_map)/launch/visualize_graph_fuser.launch" >
     <arg name="mapping" value="true" /> Command 'rosbag' from package 'python-rosbag' (universe)
-
-    <arg name="oru-lab" value="true" />
+    <arg name="coop-2013" value="true" />
   </include>
-</group>
 
 
 

graph_map/src/graph_map/map_type.cpp

@@ -47,8 +47,8 @@ void MapType::updateMap(const Eigen::Affine3d &Tnow,pcl::PointCloud<pcl::PointXY
     //octree.setInputCloud(ptr);
     //octree.addPointsFromInputCloud();
   }
-  else
-    cout<<"Mapping disabled"<<endl;
+  /*else
+    cout<<"Mapping disabled"<<endl;*/
 
 }
 void MapType::updateMap(const Eigen::Affine3d &Tnow,pcl::PointCloud<velodyne_pointcloud::PointXYZIR> &cloud, bool simple){

graph_map/src/graph_map_fuser_node.cpp

@@ -279,10 +279,12 @@ public:
       if(useOdometry) {
        {
           if(!use_tf_listener_){
+            points2_sub_ = new message_filters::Subscriber<velodyne_msgs::VelodyneScan>(nh_,points_topic,2);
             cout<<"Using synchronized odometry and velodyne messages"<<endl;
             odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh_,odometry_topic,10);
             sync_po_ = new message_filters::Synchronizer< PointsOdomSync >(PointsOdomSync(SYNC_FRAMES), *points2_sub_, *odom_sub_);
             sync_po_->registerCallback(boost::bind(&GraphMapFuserNode::points2OdomCallback, this, _1, _2));
+            cout<<"Using synchronized odometry and velodyne messages"<<endl;
           }
           else{
             if(use_pointcloud){
@@ -505,7 +507,7 @@ public:
   }
 
   void pointcloudCallbackTf(const sensor_msgs::PointCloud2::ConstPtr& msg_in){
-    cout<<"callback with tf"<<endl;
+
     pcl::PointCloud<pcl::PointXYZ> cloud;
     ros::Time t=msg_in->header.stamp;
     pcl::fromROSMsg (*msg_in, cloud);

ndt_calibration/src/ndt_calib.cpp

@@ -247,7 +247,7 @@ bool NDTCalibOptimize::calibrate(Eigen::Affine3d &Ts, double &sensorTimeOffset)
           Eigen::AngleAxis<double>(cez,Eigen::Vector3d::UnitZ()) ;
       geometry_msgs::Pose Tsensor_msg;
       tf::poseEigenToMsg(Tsens,Tsensor_msg);
-      std::cout<<"publish: "<<Tsens.translation().transpose()<<std::endl;
+      //std::cout<<"publish: "<<Tsens.translation().transpose()<<std::endl;
       sensor_pub.publish(Tsensor_msg);