Skip to content

GitLab

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

added motion compensation

parent d50de1da
Hide whitespace changes
Inline Side-by-side
Showing with 362 additions and 139 deletions
graph_localization/launch/3d_mcl_ncfm2018.launch
View file @ cf270117
......@@ -6,10 +6,16 @@
<arg name="publish_map" default="false" />
<!-- Absolute Path to map to be used for localization. The map needs to be built by graph_fuser package -->
<arg name="map_file_path" default="$(find graph_map)/maps/ndt_map.MAP" />
<arg name="robot_prefix" default="robot5" />
<arg name="robot_id" default="robot5" />
<arg name="rviz_enabled" default="false" />
<arg name="root_id" default="/map_laser2d" />
<!--include file="$(find graph_map)/launch/VelodyneConvertMotionCompensation.launch" >
<arg name="robot_id" value="$(arg robot_id)" />
<arg name="input_points_topic" value="$(arg robot_id)/sensors/velodyne_packets" />
<arg name="output_points_topic" value="$(arg robot_id)/sensors/velodyne_points_unwarped" />
</include-->
<!-- Include visualization in rviz -->
<group if="$(arg rviz_enabled)">
<include file="$(find graph_map)/launch/visualize_graph_fuser.launch" >
......@@ -25,14 +31,14 @@
<param name="Velodyne" value="true" />
<param name="PointCloud" value="false" />
<param name="dataset" value="ncfm-2018" />
<!--param name="laser_tf" value="$(arg robot_prefix)/velodyne_correct" /-->
<param name="laser_tf" value="$(arg robot_prefix)/velodyne_link" />
<!--param name="laser_tf" value="$(arg robot_id)/velodyne_correct" /-->
<param name="laser_tf" value="$(arg robot_id)/velodyne_link" />
<param name="publish_sensor_link" value="false" />
<param name="undistorted_cloud_topic" value="$(arg robot_prefix)/sensors/velodyne_points_undistorted" />
<param name="pose_estimate_topic" value="$(arg robot_prefix)/mcl_pose_estimate" />
<param name="input_cloud_topic" value="$(arg robot_id)/sensors/velodyne_packets" />
<param name="pose_estimate_topic" value="$(arg robot_id)/mcl_pose_estimate" />
<!-- Topic of laser scanner -->
<param name="points_topic" value="/$(arg robot_prefix)/sensors/velodyne_packets" />
<param name="points_topic" value="/$(arg robot_id)/sensors/velodyne_packets" />
<param name="calibration" value="$(find graph_map)/config/VLP16db.yaml" />
<!-- pass path to graph_map (.map)-->
<param name="map_file" value="$(arg map_file_path)" />
......@@ -41,8 +47,8 @@
<param name="sensor_pose_z" value="0.95" />
<param name="sensor_pose_t" value="0" />
<param name="gt_topic" value="/$(arg robot_prefix)/kmo_navserver/state" />
<param name="initial_pose_topic" value="/$(arg robot_prefix)/initialpose" />
<param name="gt_topic" value="/$(arg robot_id)/kmo_navserver/state" />
<param name="initial_pose_topic" value="/$(arg robot_id)/initialpose" />
<!-- Choose weather to initiate pose to pose_init_<x,y,t> or the data of /<gt_topic> -->
<param name="initPoseFromGT" value="false" />
......@@ -55,8 +61,8 @@
<param name="root_tf" value="$(arg root_id)" />
<!-- odometry frame id-->
<param name="odom_tf" value="$(arg robot_prefix)/odom" />
<param name="base_tf" value="$(arg robot_prefix)/base_footprint" />
<param name="odom_tf" value="$(arg robot_id)/odom" />
<param name="base_tf" value="$(arg robot_id)/base_footprint" />
<!--param name="mcl_tf" value="mcl_pose_est" /-->
......
graph_localization/rviz/localization_oru_lab.rviz
View file @ cf270117
......@@ -10,13 +10,9 @@ Panels:
- /TF1/Tree1
- /Odometry1/Shape1
- /Odometry2/Shape1
- /Odometry3
- /Odometry3/Shape1
- /PoseArray1
- /Odometry4/Shape1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /NDT1
- /Map1
Splitter Ratio: 0.5
Tree Height: 815
- Class: rviz/Selection
......@@ -126,6 +122,8 @@ Visualization Manager:
robot5/velodyne_link:
Value: true
submap_node0:
Value: false
velodyne:
Value: true
world:
Value: true
......@@ -174,8 +172,9 @@ Visualization Manager:
{}
robot5/velodyne_base_link:
robot5/velodyne_link:
{}
robot5/velodyne:
robot5/velodyne:
{}
velodyne:
{}
submap_node0:
{}
......@@ -206,7 +205,7 @@ Visualization Manager:
Scale: 1
Value: true
Value: true
Enabled: true
Enabled: false
Keep: 10000
Name: Odometry
Position Tolerance: 0.100000001
......@@ -222,7 +221,7 @@ Visualization Manager:
Value: Arrow
Topic: /robot1/kmo_navserver/state
Unreliable: false
Value: true
Value: false
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Covariance:
......@@ -240,7 +239,7 @@ Visualization Manager:
Scale: 1
Value: true
Value: true
Enabled: true
Enabled: false
Keep: 10000
Name: Odometry
Position Tolerance: 0.100000001
......@@ -254,9 +253,9 @@ Visualization Manager:
Shaft Length: 0.100000001
Shaft Radius: 0.00999999978
Value: Arrow
Topic: /ndt_mcl_pose
Topic: /robot5/mcl_pose_estimate
Unreliable: false
Value: true
Value: false
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Covariance:
......@@ -282,13 +281,13 @@ Visualization Manager:
Alpha: 1
Axes Length: 1
Axes Radius: 0.100000001
Color: 0; 255; 255
Color: 255; 0; 0
Head Length: 0.100000001
Head Radius: 0.100000001
Shaft Length: 0.100000001
Shaft Radius: 0.00999999978
Value: Arrow
Topic: /robot1/kmo_navserver/odom
Topic: /robot5/mcl_pose_estimate
Unreliable: false
Value: true
- Alpha: 1
......@@ -307,51 +306,36 @@ Visualization Manager:
Topic: /ndt_mcl_localization_graph/pose_particles
Unreliable: false
Value: true
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.300000012
Color: 204; 51; 204
Scale: 1
Value: true
Value: false
- Alpha: 0.400000006
Class: ndt_rviz/NDT
Color: 204; 51; 204
Enabled: true
Keep: 100
Name: Odometry
Position Tolerance: 0.100000001
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.100000001
Color: 255; 25; 0
Head Length: 0.100000001
Head Radius: 0.100000001
Shaft Length: 0.100000001
Shaft Radius: 0.00999999978
Value: Arrow
Topic: /ndt_mcl_pose
History Length: 1
Name: NDT
Topic: /graph_map_publisher/NDTOmMap
Unreliable: false
Value: true
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 5.9265604
Min Value: -1.14259744
Value: false
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
......@@ -364,32 +348,13 @@ Visualization Manager:
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.0500000007
Style: Points
Topic: /robot5/sensors/velodyne_points_undistorted
Size (m): 0.0299999993
Style: Flat Squares
Topic: /robot5/sensors/velodyne_points_unwarped
Unreliable: false
Use Fixed Frame: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.400000006
Class: ndt_rviz/NDT
Color: 204; 51; 204
Enabled: true
History Length: 1
Name: NDT
Topic: /graph_map_publisher/NDTOmMap
Unreliable: false
Value: true
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Global Options:
Background Color: 130; 130; 130
......@@ -414,26 +379,21 @@ Visualization Manager:
Value: true
Views:
Current:
Class: rviz/XYOrbit
Distance: 29.9753094
Angle: 1.50500131
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -4.57656956
Y: -19.4551601
Z: 1.30315848e-05
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.844796181
Scale: 32.0815163
Target Frame: world
Value: XYOrbit (rviz)
Yaw: 4.41724014
Value: TopDownOrtho (rviz)
X: -3.27640963
Y: -6.37991905
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......
graph_localization/src/mcl_ndt/mcl_ndt.cpp
View file @ cf270117
......@@ -204,6 +204,7 @@ bool MCLNDTType::UpdateAndPredict(pcl::PointCloud<pcl::PointXYZ> &cloud, const E
SetPose(Tinit);
if(visualize_){
geometry_msgs::PoseArray particles_msg= ParticlesToMsg(graph_map_->GetCurrentNodePose(), pf.pcloud);
particles_msg.header.frame_id="map_laser2d";
part_pub.publish(particles_msg);
}
updated=true;
......
graph_localization/src/ndt_mcl_localization.cpp
View file @ cf270117
......@@ -56,7 +56,7 @@ class localisation_node {
std::string initType;
ros::Publisher mclPosePub;
//laser input
std::string points_topic, gt_topic; //std::string laserTopicName;
std::string input_cloud_topic, gt_topic; //std::string laserTopicName;
ros::Publisher cloud_pub;
ros::Subscriber initPoseSub;
......@@ -180,7 +180,8 @@ class localisation_node {
}
void VeloCallback(const velodyne_msgs::VelodyneScan::ConstPtr &msg){
pcl::PointCloud<pcl::PointXYZ> cloud;
ros::Time t0=ros::Time::now();
ros::Time t_cloud_end=msg->packets[msg->packets.size()-1].stamp;
for(size_t next = 0; next < msg->packets.size(); ++next){
velodyne_rawdata::VPointCloud pnts;
......@@ -194,13 +195,15 @@ class localisation_node {
}
pnts.clear();
}
std::cout<<"Velodyne Callback :"<<msg->header.stamp<<", size="<<cloud.size()<<std::endl;
this->processFrame(cloud,t_cloud_end);
}
void PCCallback(const sensor_msgs::PointCloud2::ConstPtr& msg){
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl::fromROSMsg (*msg, pcl_cloud);
this->processFrame(pcl_cloud,msg->header.stamp);
std::cout<<"Callback :"<<msg->header.stamp<<", size="<<pcl_cloud.size()<<std::endl;
this->processFrame(pcl_cloud, msg->header.stamp);
}
void LaserCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
......@@ -297,12 +300,12 @@ public:
param.param<std::string>("map_file", map_path, "");
param.param<bool>("visualize", visualize, true);
param.param<std::string>("points_topic", points_topic, "/velodyne_packets");
param.param<std::string>("input_cloud_topic", input_cloud_topic, "/velodyne_packets");
param.param<std::string>("gt_topic", gt_topic, "");
param.param<std::string>("pose_estimate_topic", pose_estimate_topic, "/ndt_mcl_pose");
param.param<bool>("initPoseFromGT", init_pose_gt, true);
param.param<bool>("Velodyne", beVelodyne, false);
param.param<bool>("PointCloud", bePC, false);
param.param<bool>("PointCloud", bePC, true);
param.param<bool>("Laser_2d", laser_2d, false);
param.param<std::string>("root_tf", rootTF, "/map");
......@@ -311,7 +314,7 @@ public:
param.param<std::string>("mcl_tf", mclTF, "/mcl");
param.param<std::string>("laser_tf", laser_link_id, "/velodyne");
param.param<bool>("publish_sensor_link", publish_sensor_link, false);
param.param<string>("undistorted_cloud_topic", undistorted_cloud_topic, "cloud_undistorted");
Eigen::Vector3d sensor_offset_pos, sensor_offset_euler;
param.param("sensor_pose_x",sensor_offset_pos(0),0.);
......@@ -371,19 +374,20 @@ public:
if(beVelodyne){
PCSub = nh.subscribe(points_topic, 1, &localisation_node::VeloCallback, this);
cout<<"Listen to sensor_msgs::PointCloud2 at topic \""<<points_topic<<"\""<<endl;
PCSub = nh.subscribe(input_cloud_topic, 1, &localisation_node::VeloCallback, this);
cout<<"Listen to sensor_msgs::velodyne_msgs::VelodyneScan at topic \""<<input_cloud_topic<<"\""<<endl;
}
else if(bePC){
PCSub = nh.subscribe(points_topic, 1, &localisation_node::PCCallback, this);
cout<<"Listen to sensor_msgs::velodyne_msgs::VelodyneScan at topic \""<<points_topic<<"\""<<endl;
PCSub = nh.subscribe(input_cloud_topic, 1, &localisation_node::PCCallback, this);
cout<<"Listen to sensor_msgs::PointCloud2 at topic \""<<input_cloud_topic<<"\""<<endl;
}
else if(laser_2d){
PCSub = nh.subscribe(points_topic, 1, &localisation_node::LaserCallback, this);
cout<<"Listen to 2d-laser at topic \""<<points_topic<<"\""<<endl;
PCSub = nh.subscribe(input_cloud_topic, 1, &localisation_node::LaserCallback, this);
cout<<"Listen to 2d-laser at topic \""<<input_cloud_topic<<"\""<<endl;
}
else
std::cerr<<"No lidar Type sepected for topic \""<<points_topic<<"\""<<std::endl;
std::cerr<<"No lidar Type sepected for topic \""<<input_cloud_topic<<"\""<<std::endl;
initPoseSub = nh.subscribe("/initialpose", 1000, &localisation_node::initialposeCallback, this);
......@@ -398,7 +402,7 @@ public:
}
mclPosePub=nh.advertise<nav_msgs::Odometry>(pose_estimate_topic, 20);
cloud_pub=nh.advertise<pcl::PointCloud<pcl::PointXYZ>>(undistorted_cloud_topic, 20);
cloud_pub=nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("undistorted_cloud_topic", 20);
ros::spin();
}
};
......
graph_map/CMakeLists.txt
View file @ cf270117
......@@ -109,6 +109,9 @@ add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
#add_executable(test_grid test/test_grid.cpp )
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} pcl_common pcl_octree)
add_executable(velodyne_converter src/VelodyneMotionCompensation.cpp )
target_link_libraries(velodyne_converter ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
add_executable(show_map src/show_map.cpp )
target_link_libraries(show_map ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} )
......
graph_map/launch/VelodyneConvertMotionCompensation.launch 0 → 100644
View file @ cf270117
<?xml version="1.0"?>
<launch>
<arg name="robot_id" default="robot5" />
<arg name="input_points_topic" default="/$(arg robot_id)/sensors/velodyne_packets" />
<arg name="output_points_topic" default="/$(arg robot_id)/sensors/velodyne_points_unwarped" />
<!-- Run monte carlo localization -->
<node name="velodyne_converter" pkg="graph_map" type="velodyne_converter" output="screen">
<!-- select one of the following types-->
<param name="disable_compensation" value="false" />
<param name="laser_tf" value="$(arg robot_id)/velodyne_link" />
<!-- Topic of laser scanner -->
<param name="input_points_topic" value="$(arg input_points_topic)" />
<param name="output_points_topic" value="$(arg output_points_topic)" />
<param name="output_points_link" value="/$(arg robot_id)/velodyne_link" />
<param name="calibration" value="$(find graph_map)/config/VLP16db.yaml" />
<param name="sensor_offset_topic" value="/sensor_pose" />
<param name="odom_parent_link" value="$(arg robot_id)/odom" />
<param name="odom_link" value="$(arg robot_id)/base_footprint" />
<!-- pass path to graph_map (.map)-->
<param name="sensor_pose_x" value="0.80" />
<param name="sensor_pose_y" value="0" />
<param name="sensor_pose_z" value="0.95" />
<param name="sensor_pose_t" value="0" />
</node>
</launch>
graph_map/src/VelodyneMotionCompensation.cpp 0 → 100644
View file @ cf270117
//perception_oru
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
//pcl
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
//ros
#include "geometry_msgs/PoseArray.h"
#include "laser_geometry/laser_geometry.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "ros/rate.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include "pcl_ros/point_cloud.h"
#include "pcl_ros/transforms.h"
#include <velodyne_pointcloud/rawdata.h>
#include <velodyne_pointcloud/point_types.h>
#include "eigen_conversions/eigen_msg.h"
#include <pcl_ros/impl/transforms.hpp>
#include "ndt_generic/pcl_utils.h"
#include "ndt_generic/eigen_utils.h"
#include <pcl/conversions.h>
//std
#include <Eigen/Dense>
#include <string>
#include <map>
#include <fstream>
#include <iostream>
#include <chrono>
#include "ndt_generic/pcl_utils.h"
#include "ndt_map/pointcloud_utils.h"
#include "eigen_conversions/eigen_msg.h"
#include "tf_conversions/tf_eigen.h"
class localisation_node {
ros::NodeHandle nh;
//Map parameters
//MCL
ros::Publisher cloud_pub;
ros::Subscriber cloud_sub;
ros::Subscriber sensor_offset_sub;
std::string input_points_topic, output_points_topic, sensor_offset_topic;
std::string odom_parent_link, odom_link, output_points_link;
tf::TransformBroadcaster trans_pub;
velodyne_rawdata::RawData dataParser;
tf::TransformListener tf_listener;
double min_range, max_range;
Eigen::Affine3d Tsensor;
bool disable_compensation;
void SensorOffsetCallback(const geometry_msgs::PoseConstPtr &msg){
//tf::poseMsgToTF(*msg, Tsensor);:
tf::poseMsgToEigen(*msg, Tsensor);
ROS_INFO("changed sensor offset wrt. odometry link");
}
void VeloCallback(const velodyne_msgs::VelodyneScan::ConstPtr &msg){
ros::Time ts=msg->header.stamp;
pcl::PointCloud<pcl::PointXYZ> cloud;
bool unwarped=true;
if(disable_compensation)
ndt_generic::UnwarpCloudSimple(dataParser,msg,cloud);
else
unwarped=this->OdometryTFaUnwarpTransform(msg, cloud);
//if(unwarped){
cloud.header.frame_id = output_points_link;
cloud.height=1;
cloud.width=cloud.size();
std::cout<<"Sending"<<msg->header.stamp<<", sending cloud size="<<cloud.size()<<std::endl;
cloud_pub.publish(cloud);
//}
}
bool Transform(const ros::Time &ts, Eigen::Affine3d &T){
tf::StampedTransform transform;
tf_listener.waitForTransform(odom_parent_link, odom_link, ts, ros::Duration(0.1));
try{
tf_listener.lookupTransform(odom_parent_link, odom_link, ts, transform);
}
catch(tf::TransformException ex){
ROS_ERROR("%s", ex.what());
return false;
}
tf::poseTFToEigen(transform, T);
return true;
}
void convertPointCloud(const velodyne_rawdata::VPointCloud &conv_points,
pcl::PointCloud<pcl::PointXYZ> &cloud) {
for(size_t i = 0;i<conv_points.size();i++){
pcl::PointXYZ p;
p.x =conv_points.points[i].x; p.y=conv_points.points[i].y; p.z=conv_points.points[i].z;
cloud.push_back(p);
}
}
bool OdometryTFaUnwarpTransform(const velodyne_msgs::VelodyneScan::ConstPtr &msg, pcl::PointCloud<pcl::PointXYZ> &cloud){
ros::Time t_first=msg->packets[0].stamp, t_last=msg->packets[msg->packets.size()-1].stamp;
Eigen::Affine3d tf_first, tf_last;
Eigen::Affine3d Todom_first, Todom_last;
if(!Transform(t_first, Todom_first) || !Transform(t_last, Todom_last)){ //tf exists for first and last
std::cout<<"Unwarp without interpolation"<<std::endl;
ndt_generic::UnwarpCloudSimple(dataParser, msg, cloud);
cloud.header.frame_id = output_points_link;
pcl_conversions::toPCL(t_last, cloud.header.stamp);
return false;
}
Eigen::Affine3d Tsens_last=Todom_last*Tsensor;
for (size_t next = 0; next < msg->packets.size(); ++next){
velodyne_rawdata::VPointCloud pnts,conv_points;
dataParser.unpack(msg->packets[next], pnts); // unpack the raw data
ros::Time t_pkg_i=msg->packets[next].stamp;
Eigen::Affine3d Todom_i;
Transform(t_pkg_i, Todom_i);
Eigen::Affine3d Tsensor_tnow=Todom_i*Tsensor;
Eigen::Affine3d Tmotion_sensor=Tsens_last.inverse()*Tsensor_tnow;
tf::Transform Tmot_tf;
tf::poseEigenToTF(Tmotion_sensor, Tmot_tf);
pcl_ros::transformPointCloud(pnts, conv_points, Tmot_tf);
convertPointCloud(conv_points, cloud);
pnts.clear();
conv_points.clear();
}
cloud.header.frame_id = output_points_link;
pcl_conversions::toPCL(t_last, cloud.header.stamp);
cloud.width=cloud.size();
cloud.height=1;
return true;
}
public:
localisation_node(ros::NodeHandle param){
param.param<bool>("disable_compensation", disable_compensation, "false");
param.param<std::string>("input_points_topic", input_points_topic, "/velodyne_packets");
param.param<std::string>("sensor_offset_topic", sensor_offset_topic, "/offset");
param.param<std::string>("odom_parent_link", odom_parent_link, "/odom");
param.param<std::string>("odom_link", odom_link, "/base_link");
param.param<std::string>("output_points_link", output_points_link, "/velodyne");
param.param<std::string>("output_points_topic", output_points_topic, "cloud_undistorted");
Eigen::Vector3d sensor_offset_pos, sensor_offset_euler;
param.param("sensor_pose_x",sensor_offset_pos(0),0.);
param.param("sensor_pose_y",sensor_offset_pos(1),0.);
param.param("sensor_pose_z",sensor_offset_pos(2),0.);
param.param("sensor_pose_r",sensor_offset_euler(0),0.);
param.param("sensor_pose_p",sensor_offset_euler(1),0.);
param.param("sensor_pose_t",sensor_offset_euler(2),0.);
Tsensor = ndt_generic::vectorsToAffine3d(sensor_offset_pos,sensor_offset_euler);
param.param("min_range", min_range, 1.3);
param.param("max_range", max_range, 130.0);
int retu = dataParser.setup(param);