Commit ca204539 authored by dla.adolfsson@gmail.com's avatar dla.adolfsson@gmail.com
Browse files
parents ea6c2321 ac26c563
# Compiled source #
###################
*.com
*.class
*.dll
*.exe
*.o
*.so
# Packages #
############
# it's better to unpack these files and commit the raw source
# git has its own built in compression methods
*.7z
*.dmg
*.gz
*.iso
*.jar
*.rar
*.tar
*.zip
# Logs and databases #
######################
*.log
*.sql
*.sqlite
# OS generated files #
######################
.DS_Store
.DS_Store?
._*
.Spotlight-V100
.Trashes
ehthumbs.db
Thumbs.
# Models #
*.pyc
*.meta
*.ckpt*
#Documentation
documentation/
*~
\ No newline at end of file
stages:
- dev-build
- night-build
- release-build
# Ubuntu 16.04
ubuntu-xenial-test:
image: ubuntu:xenial
variables:
CI_ROS_DISTRO: kinetic
UBUNTU_DISTRO: xenial
before_script:
- echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
- apt-get update
- apt-get install -qq -y apt-transport-https sed wget unzip > /dev/null
- source .gitlab-ci/prepare.sh
- chmod +x setup.sh
- ./setup.sh $UBUNTU_DISTRO $CI_ROS_DISTRO > /dev/null
- setup_apt ${UBUNTU_DISTRO}
- apt-get install -qq -y python-catkin-tools > /dev/null
- rosdep update 2> /dev/null
- add_aass_yaml
- rosdep install --from-paths . -i -y -r --rosdistro $CI_ROS_DISTRO 2> /dev/null
stage: dev-build
script:
- old_dir=`pwd`
- mkdir -p /tmp/src
- cp -r * /tmp/src/
- cd /tmp/
- source /opt/ros/${CI_ROS_DISTRO}/setup.sh
- catkin init
- catkin build
- cd $old_dir
- rm -rf /tmp/src
# Ubuntu 16.04 debian package generation
ubuntu-xenial-build-package-nightbuild:
image: ubuntu:xenial
variables:
CI_ROS_DISTRO: kinetic
UBUNTU_DISTRO: xenial
before_script:
- echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
- echo $CI_ROS_DISTRO
- echo ${CI_ROS_DISTRO}
- apt-get update
- apt-get install -qq -y apt-transport-https sed wget unzip > /dev/null
- source .gitlab-ci/prepare.sh
- chmod +x setup.sh
- ./setup.sh $UBUNTU_DISTRO $CI_ROS_DISTRO > /dev/null
- setup_apt ${UBUNTU_DISTRO}
- apt-get install -q -y python-bloom devscripts debhelper > /dev/null
- apt-get install -f -y > /dev/null
- rosdep update 2> /dev/null
- add_aass_yaml
- rosdep install --from-paths . -i -y -r --rosdistro $CI_ROS_DISTRO 2> /dev/null
stage: night-build
script:
- ls *
- rm -rf build
- source .gitlab-ci/release.sh
- show_info
- mkdir .build
- release_package
- mv .build build
when: on_success
# The files which are to be made available in GitLab
artifacts:
paths:
- build/*
# Ubuntu 16.04 debian package generation
ubuntu-xenial-build-package-release:
image: ubuntu:xenial
variables:
CI_ROS_DISTRO: kinetic
UBUNTU_DISTRO: xenial
before_script:
- echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
- echo $CI_ROS_DISTRO
- echo ${CI_ROS_DISTRO}
- apt-get update
- apt-get install -qq -y apt-transport-https sed wget unzip > /dev/null
- source .gitlab-ci/prepare.sh
- chmod +x setup.sh
- ./setup.sh $UBUNTU_DISTRO $CI_ROS_DISTRO > /dev/null
- setup_apt ${UBUNTU_DISTRO}
- apt-get install -q -y python-bloom devscripts debhelper > /dev/null
- apt-get install -f -y > /dev/null
- rosdep update 2> /dev/null
- rosdep install --from-paths . -i -y -r --rosdistro $CI_ROS_DISTRO 2> /dev/null
- add_ndt_core_yaml
stage: release-build
script:
- ls *
- rm -rf build
- source .gitlab-ci/release.sh
- show_info
- mkdir .build
- release_package
- mv .build build
only:
- tags
when: on_success
# The files which are to be made available in GitLab
artifacts:
paths:
- build/*
#!/bin/bash
add_aass_yaml() {
echo "Here adding aass dependencies.";
echo "yaml https://gitsvn-nt.oru.se/software/aass_rosdep/raw/master/kinetic.yml" | tee /etc/ros/rosdep/sources.list.d/50-aass.list
rosdep update > /dev/null
}
setup_apt() {
echo "Adding aass packages";
wget -O - http://apt.aass.oru.se/repos/apt/conf/public.gpg.key | apt-key add -
echo "deb http://apt.aass.oru.se/repos/apt/ubuntu xenial main" >> /etc/apt/sources.list
apt-get update > /dev/null
}
#!/bin/bash
release_dependency_packages() {
for i in graph_map graph_localization ndt_calibration ndt_evaluation; do
create_debian_package "$i"
dpkg -i ros-*_amd64.deb > /dev/null
done
move_debian_packages
}
release_meta_package() {
create_debian_package "<meta package>"
dpkg -i ros-*_amd64.deb > /dev/null
move_debian_packages
}
create_debian_package() {
cd $1
if [ -f .done ]; then
echo "Package $1 was already build. Skipping..."
else
echo "generating debian package for $1"
bloom-generate rosdebian --os-name ubuntu --os-version $UBUNTU_DISTRO --ros-distro $CI_ROS_DISTRO &&\
sed -i 's/dh $@/dh $@ --parallel/' debian/rules
debuild -rfakeroot -us -uc -b -j8 > /dev/null
touch .done
fi
cd ..
}
move_debian_packages() {
mv *deb .build/
}
release_package() {
release_dependency_packages
#release_meta_package
}
show_info() {
echo $UBUNTU_DISTRO
echo $CI_ROS_DISTRO
}
# Releasing your code via aass build farm
## Repository preparation
1. Copy content of this repository into the root directory of your package
2. Modify line 13 in .gitlab-ci/release.sh. Replace <meta package> with the name of your meta package.
3. Modify line 4 in .gitlab-ci/release.sh. Replace <Topological order of packages> with the topological order of packages in your meta package. (You can use catkin_topological_order to check it.)
4. In tab Setting->General of your project add tag ros
## Add dependencies
1. Add all your packages to the list in https://gitsvn-nt.oru.se/software/aass_rosdep.
\ No newline at end of file
......@@ -46,6 +46,7 @@ protected:
Eigen::Affine3d pose_last_update_;
std::vector<double> motion_model, motion_model_offset;
MapSwitchingMethod switch_map_method_;
private:
friend class LocalisationFactory;
......
......@@ -12,6 +12,7 @@ Panels:
- /Odometry2/Shape1
- /Odometry3/Shape1
- /PointCloud22/Autocompute Value Bounds1
- /Marker1
- /NDT1
Splitter Ratio: 0.5
Tree Height: 793
......@@ -83,6 +84,18 @@ Visualization Manager:
Value: true
submap_node1:
Value: true
submap_node10:
Value: true
submap_node11:
Value: true
submap_node12:
Value: true
submap_node13:
Value: true
submap_node14:
Value: true
submap_node15:
Value: true
submap_node2:
Value: true
submap_node3:
......@@ -97,6 +110,8 @@ Visualization Manager:
Value: true
submap_node8:
Value: true
submap_node9:
Value: true
velodyne:
Value: true
world:
......@@ -120,7 +135,14 @@ Visualization Manager:
submap_node6:
submap_node7:
submap_node8:
{}
submap_node9:
submap_node10:
submap_node11:
submap_node12:
submap_node13:
submap_node14:
submap_node15:
{}
Update Interval: 0
Value: true
- Angle Tolerance: 0.100000001
......@@ -353,7 +375,7 @@ Visualization Manager:
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /graph_fuser3d_offline/graph_info
Marker Topic: /localisation_1/graph_info
Name: Marker
Namespaces:
graphInfo: true
......@@ -365,7 +387,7 @@ Visualization Manager:
Enabled: true
History Length: 10
Name: NDT
Topic: /graph_fuser3d_offline/l2_measure
Topic: /localisation_1/NDTOmMap
Unreliable: false
Value: true
Enabled: true
......@@ -393,25 +415,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 11.893507
Distance: 9.76567554
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 2.2927053
Y: -1.78755701
Z: 0.542825639
X: -0.699659169
Y: 1.42053759
Z: 4.39885855
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.12539876
Pitch: 1.44539881
Target Frame: fuser
Value: Orbit (rviz)
Yaw: 3.45042205
Yaw: 2.75858712
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......
roslaunch graph_map visualize_graph_fuser.launch localization:=true arla-2012:=true &
rosrun graph_localization graph_localization_offline --visualize --visualize-map --map-switching-method overlap --keyframe-min-distance 0.2 --keyframe-min-rot-deg 2 --key-frame-update --disable-unwarp --tf-base-link /odom_base_link --tf-gt-link /state_base_link --forceSIR --n-particles 450 --localisation-algorithm-name reg_localisation_type --skip-frame 1 --base-name mcl-ndt --bag-file-path $BAG_LOCATION/arla_bags/mapping/2012-09-05-09-44-50_0.bagfix.bag_edited.bag --map-file-path /home/daniel/.ros/maps/arla/offarla-2012_gt=1_submap=1_sizexy=120_Z=15_intrchR=10_compR=0_res=0.4_maxSensd=130_keyF=1_d=0.6_deg=5_alpha=0_dl=0_xyzir=0_mpsu=0_mnpfg=6kmnp0.map --data-set arla-2012 --velodyne-config-file "$(rospack find graph_map)/config/velo32.yaml" --z-filter-height -1000000 resolution-local-factor 1.5 #--save-results
rosrun graph_localization graph_localization_offline --step-control --map-switching-method overlap_registration --keyframe-min-distance 0.2 --keyframe-min-rot-deg 2 --key-frame-update --disable-unwarp --tf-base-link /odom_base_link --tf-gt-link /state_base_link --localisation-algorithm-name reg_localisation_type --skip-frame 1 --base-name mcl-ndt --bag-file-path $BAG_LOCATION/arla_bags/mapping/2012-09-05-09-44-50_0.bagfix.bag_edited.bag --map-file-path /home/daniel/.ros/maps/arla/esg_small.map --data-set arla-2012 --velodyne-config-file "$(rospack find graph_map)/config/velo32.yaml" resolution-local-factor 1.5 #--save-results
......@@ -307,7 +307,7 @@ void ReadAllParameters(po::options_description &desc,int &argc, char ***argv){
save_eval_results=vm.count("save-results");
visualize = vm.count("visualize");
visualize_map = vm.count("visualize-map");
step_control = vm.count("step-cvisualizeontrol");
step_control = vm.count("step-control");
filter_fov = vm.count("filter-fov");
filter_ring_nb = vm.count("filter-ring-nb");
use_pointtype_xyzir = vm.count("use_pointtype_xyzir");
......@@ -334,7 +334,7 @@ void ReadAllParameters(po::options_description &desc,int &argc, char ***argv){
localisation_param_ptr->switch_map_method=GraphMapNavigatorParam::String2SwitchMethod(map_switching_method);
localisation_param_ptr->n_obs_search=n_obs_search;
localisation_param_ptr->enable_localisation=!vm.count("disable-localization");
cout<<"will use method: "<<GraphMapNavigatorParam::SwitchMethod2String(localisation_param_ptr->switch_map_method)<<endl;
cout<<"will use method: "<<map_switching_method<<endl;
if(MCLNDTParamPtr parPtr=boost::dynamic_pointer_cast<MCLNDTParam>(localisation_param_ptr )){
parPtr->n_particles=n_particles;
parPtr->z_filter_min=z_filter_min_height;
......@@ -413,12 +413,12 @@ void processData() {
localisation_param_ptr->graph_map_=graph_map_;
localisation_type_ptr=LocalisationFactory::CreateLocalisationType(localisation_param_ptr);
if(graph_map_==NULL ||localisation_type_ptr==NULL){
cout<<"problem opening map"<<endl;
exit(0);
}
vis=new graphVisualization(graph_map_,visualize,visualize_map,true);
vis->SetParameters(10,0.5);
......@@ -427,9 +427,9 @@ void processData() {
cout<<"--------------------------------------------------------"<<endl;
if(localisation_type.compare("reg_localisation_type")==0)
output_file_name = ndt_generic::removeExtension(map_file_path)+localisation_type+"_lkeyd="+toString(min_keyframe_dist)+"_lkeydeg="+toString(min_keyframe_rot_deg)+"_attempt="+toString(attempt);
output_file_name = ndt_generic::removeExtension(map_file_path)+localisation_type+"_lkeyd="+toString(min_keyframe_dist)+"_lkeydeg="+toString(min_keyframe_rot_deg)+"_attempt="+toString(attempt)+"_"+map_switching_method;
else
output_file_name = ndt_generic::removeExtension(map_file_path)+localisation_type+"_npart="+toString(n_particles)+"_mpsu="+toString(min_nb_points_set_uniform)+"_mnpfg="+toString(min_nb_points_for_gaussian)+"_attempt="+toString(attempt);
output_file_name = ndt_generic::removeExtension(map_file_path)+localisation_type+"_npart="+toString(n_particles)+"_mpsu="+toString(min_nb_points_set_uniform)+"_mnpfg="+toString(min_nb_points_for_gaussian)+"_attempt="+toString(attempt)+"_"+map_switching_method;
ndt_generic::CreateEvalFiles eval_files(output_dir_name,output_file_name,save_eval_results);
int counter = 0;
......@@ -443,7 +443,7 @@ void processData() {
t5=ros::Time::now();
bool no_update=false;
bool initialized=false;
while(reader.readNextMeasurement(cloud)){
while(reader.readNextMeasurement(cloud) && ros::ok()){
counter ++;
Tgt_base=Todom_base=Eigen::Affine3d::Identity();
//cout<<"process frame"<<endl;
......
......@@ -17,6 +17,7 @@ RegLocalisationType::RegLocalisationType(LocalisationParamPtr param):Localizatio
min_keyframe_dist_=loc_param_ptr->min_keyframe_dist;
min_keyframe_dist_rot_deg_=loc_param_ptr->min_keyframe_dist_rot_deg;
graph_map_->SetMapSwitchMethod(param->switch_map_method);
switch_map_method_ = param->switch_map_method;
if(NDTD2DRegParamPtr ndt_reg_par=boost::dynamic_pointer_cast<NDTD2DRegParam>(reg_par_ptr)){
ndt_reg_par->resolution=boost::dynamic_pointer_cast<NDTMapType>(graph_map_->GetCurrentNode()->GetMap())->GetResolution();
......@@ -45,22 +46,21 @@ bool RegLocalisationType::UpdateAndPredict(pcl::PointCloud<pcl::PointXYZ> &cloud
perform_registration=true;
if(perform_registration){
/*Eigen::Affine3d Tsensor=Tinit*sensor_pose_;
MapNodePtr nodePtr = graph_map_->GetMapByRegistration(Tsensor, cloud, regptr_);
graph_map_->SwitchToMapNode(nodePtr);
Tinit=Tsensor*sensor_pose_.inverse();
pose_last_update_=Tinit;
SetPose(Tinit);*/
graph_map_->WorldToLocalMapFrame(Tinit);
bool successful=regptr_->RegisterScan(graph_map_->GetCurrentNode()->GetMap(), Tinit, cloud);
graph_map_->LocalToWorldMapFrame(Tinit);
if(successful)
if(switch_map_method_==overlap_registration){
Eigen::Affine3d Tsensor=Tinit*sensor_pose_;
MapNodePtr nodePtr = graph_map_->GetMapByRegistration(Tsensor, cloud, regptr_);
graph_map_->SwitchToMapNode(nodePtr);
Tinit=Tsensor*sensor_pose_.inverse();
pose_last_update_=Tinit;
graph_map_->SwitchToClosestMapNode(Tinit*sensor_pose_, cloud, DBL_MAX); //Select map before registration
}
else{
graph_map_->WorldToLocalMapFrame(Tinit);
bool successful=regptr_->RegisterScan(graph_map_->GetCurrentNode()->GetMap(), Tinit, cloud);
graph_map_->LocalToWorldMapFrame(Tinit);
if(successful)
pose_last_update_=Tinit;
graph_map_->SwitchToClosestMapNode(Tinit*sensor_pose_, cloud, DBL_MAX); //Select map before registration
}
SetPose(Tinit);
return true;
}
......
......@@ -108,11 +108,11 @@ protected:
RegTypePtr registrator_;
MotionModel3d motion_model_3d_;
plotmarker marker_;
ndt_generic::PointCloudQueue<pcl::PointXYZ> cloud_queue_;
bool initialized_=false;
bool visualize_=false;
unsigned int nr_frames_=0;
bool use_keyframe_=true;
double min_keyframe_dist_=0.5;
double min_keyframe_rot_deg_=15;
......
......@@ -14,8 +14,6 @@ bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d
*/
template<class PointT>
bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d &Tnow, Eigen::Affine3d &Tmotion, Eigen::MatrixXd &Tcov){
if(use_scanmatching)
return scanmatching(cloud,Tnow,Tmotion);
if(!initialized_){
cerr<<"fuser not initialized"<<endl;
......@@ -39,19 +37,10 @@ bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d
if(graph_map_->GetCurrentNode()->GetMap()->Initialized()){
graph_map_->WorldToLocalMapFrame(Tnow);
registration_succesfull = registrator_->RegisterScan(graph_map_->GetCurrentNode()->GetMap(),Tnow,cloud,cov);//Tnow will be updated to the actual pose of the robot according to ndt-d2d registration
/* if(registration_succesfull){
std::string node_link_id=GetNodeLinkName( graph_map_->GetCurrentNode());
NDTMap *scan_ndt=boost::dynamic_pointer_cast<NDTD2DRegType>(registrator_)->GetRegisteredMap();
pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
pcl::copyPointCloud(cloud, cloud_xyz);
GraphPlot::PlotNDT(scan_ndt, node_link_id, "ndt_scan_registeted",Tnow);
GraphPlot::PlotScan(cloud_xyz, node_link_id, "scan_registered", Tnow);
}*/
graph_map_->LocalToWorldMapFrame(Tnow);
}
Eigen::Affine3d Tsensor_pose;
Tsensor_pose=Tnow*sensorPose_;
graph_map_->AutomaticMapInterchange(Tsensor_pose,motion_cov,map_node_changed,map_node_created);
}
......@@ -61,12 +50,14 @@ bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d
else
UpdateSingleMap(cloud,Tnow);
if ( save_merged_cloud_){
pcl::PointCloud<pcl::PointXYZ> tmp_cloud;
copyPointCloud(cloud, tmp_cloud);
cloud_queue_.Push(tmp_cloud);
/* if ( save_merged_cloud_){
Eigen::Affine3d sens_in_world=Tnow*sensorPose_;
pcl::PointCloud<pcl::PointXYZ> tmp_transformed;
transformPointCloudInPlace(sens_in_world, tmp);
copyPointCloud(tmp,tmp_transformed);
cloud_queue_.Push(tmp_transformed);
cloud_queue_.Save();
}
}*/
graph_map_->m_graph.unlock();
nr_frames_++;
......
......@@ -18,7 +18,7 @@ namespace perception_oru{
namespace graph_map{
using namespace perception_oru;
typedef enum SelectMap{node_position=0,mean_observation=1,closest_observation=2, grid=3,node_position_esg=4, overlap=5}MapSwitchingMethod;
typedef enum SelectMap{node_position=0,mean_observation=1,closest_observation=2, grid=3,node_position_esg=4, overlap=5, overlap_registration=6}MapSwitchingMethod;
class GraphMapNavigator:public GraphMap{
public:
......@@ -89,8 +89,8 @@ protected:
MapSwitchingMethod map_switch_method_=node_position;
//distance metrics
double alpha_=0.0;
double k_=0.0;
unsigned int n_search_=5;
double k_=1.0;
unsigned int n_search_=15;
Eigen::Affine3d Tsensor_;
private:
......
......@@ -29,11 +29,13 @@ class graphVisualization
{
public:
graphVisualization(GraphMapNavigatorPtr &graph_map, bool visualization_enabled=true, bool visualize_map=true, bool parallel_execution=false);
graphVisualization(GraphMapNavigatorPtr &graph_map, bool visualization_enabled, bool visualize_map, bool parallel_execution);
graphVisualization(GraphMapNavigatorPtr &graph_map, bool parallel_execution); // gets parameters from ros
~graphVisualization(){despawn_thread_=true; std::thread(*plot_thread);}
void SetParameters(double Tmap, double Tother){T_map=ros::Duration(Tmap); T_other=ros::Duration(Tother);}
void SetParameters(double Tmap, double Tother){T_map = Tmap; T_other = Tother;}
void PlotCurrentMap(PlotMarker marker=sphere,const ros::Time t=ros::Time::now());
......@@ -49,6 +51,8 @@ public:
void ForcePlot(){force_update_=true;}
void GetParametersFromRos();
private:
......@@ -66,7 +70,7 @@ void PlotGraphThread();
bool visualization_enabled_=false;
bool visualize_map_=false;
ros::Duration T_map=ros::Duration(5.0), T_other=ros::Duration(1.0);
double T_map = 5.0, T_other = 1.0;
bool force_update_=false;
std::thread* plot_thread ;
bool despawn_thread_=false;
......
<?xml version="1.0"?>
<launch>
<node pkg="velodyne_pointcloud" type="cloud_node" name="convert_veloscans">
<param name="calibration" value="$(find ndt_offline)/config/VLP16db.yaml" />
</node>
<param name="use_sim_time" value="false"/>
<!--node pkg="velodyne_pointcloud" type="cloud_node" name="convert_veloscans">
<param name="calibration" value="$(find graph_map)/config/VLP16db.yaml" />
</node-->
<node pkg="graph_map" type="graph_map_fuser_node" name="graph_node" output="screen">
<param name="renderGTmap" value="false" />
<param name="gt_topic" value="robot1/kmo_navserver/state" />
<param name="initPoseFromGT" value="true" />
<param name="initPoseFromGT" value="false" />
<param name="map_type" value="ndt_map" />
<param name="registration_type" value="ndt_d2d_reg" />
......@@ -25,25 +27,29 @@
<param name="check_consistency" value="true" />
<param name="tf_pose_frame" value="" />
<param name="matchLaser" value="false" />
<param name="laser_2d" value="false" />
<!--<param name="laser_topic" value="/laserscan" />-->
<param name="points_topic" value="/velodyne_points" />
<param name="points_topic" value="/velodyne_packets" />
<param name="velodyne_config_file" value="$(find graph_map)/config/VLP16db.yaml" />
<param name="useOdometry" value="true" />
<param name="odometry_topic" value="/robot1/kmo_navserver/odom" />
<param name="use_tf_listener" value="false" />
<param name="max_translation_norm" value="0.4" />
<param name="max_rotation_norm" value="0.78539816339" />
<param name="size_x_meters" value="300" />
<param name="size_y_meters" value="300" />
<param name="size_x_meters" value="100" />