Commit 48dc9245 authored by dla.adolfsson@gmail.com's avatar dla.adolfsson@gmail.com
Browse files

added backup mapping demo

parent 4ed01a15
......@@ -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 -->
......
<?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>
......
......@@ -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){
......
......@@ -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);
......
......@@ -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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment