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

changed to only optionally publish sensor link

parent 21012be8
......@@ -25,8 +25,11 @@
<param name="Velodyne" value="true" />
<param name="PointCloud" value="false" />
<param name="dataset" value="ncfm-2018" />
<param name="laser_tf" value="/velodyne" />
<param name="publish_sensor_link" value="true " />
<!--param name="laser_tf" value="$(arg robot_prefix)/velodyne_correct" /-->
<param name="laser_tf" value="$(arg robot_prefix)/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" />
<!-- Topic of laser scanner -->
<param name="points_topic" value="/$(arg robot_prefix)/sensors/velodyne_packets" />
......@@ -39,6 +42,7 @@
<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" />
<!-- Choose weather to initiate pose to pose_init_<x,y,t> or the data of /<gt_topic> -->
<param name="initPoseFromGT" value="false" />
......
......@@ -38,7 +38,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
......@@ -65,24 +65,18 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
asus_fork_depth_frame:
Value: true
asus_fork_depth_optical_frame:
Value: true
asus_fork_link:
Value: true
asus_fork_rgb_frame:
Value: true
asus_fork_rgb_optical_frame:
Value: true
map_laser2d:
Value: true
robot5/asus_fork_base_link:
Value: true
robot5/asus_fork_depth_frame:
Value: true
robot5/asus_fork_depth_optical_frame:
Value: true
robot5/asus_fork_link:
Value: true
robot5/asus_fork_rgb_frame:
Value: true
robot5/asus_fork_rgb_optical_frame:
Value: true
robot5/base_footprint:
......@@ -133,8 +127,6 @@ Visualization Manager:
Value: true
submap_node0:
Value: true
velodyne:
Value: true
world:
Value: true
Marker Scale: 1
......@@ -149,10 +141,12 @@ Visualization Manager:
robot5/base_link:
robot5/asus_fork_base_link:
robot5/asus_fork_link:
robot5/asus_fork_depth_optical_frame:
{}
robot5/asus_fork_rgb_optical_frame:
{}
robot5/asus_fork_depth_frame:
robot5/asus_fork_depth_optical_frame:
{}
robot5/asus_fork_rgb_frame:
robot5/asus_fork_rgb_optical_frame:
{}
robot5/base_fork:
robot5/left_fork:
{}
......@@ -180,15 +174,11 @@ Visualization Manager:
{}
robot5/velodyne_base_link:
robot5/velodyne_link:
robot5/velodyne:
{}
velodyne:
{}
robot5/velodyne:
{}
submap_node0:
{}
world:
robot5/base_footprint_ground_truth:
{}
Update Interval: 0
Value: true
- Class: rviz/MarkerArray
......@@ -376,7 +366,7 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.0500000007
Style: Points
Topic: /cloud_localized
Topic: /robot5/sensors/velodyne_points_undistorted
Unreliable: false
Use Fixed Frame: false
Use rainbow: true
......@@ -425,25 +415,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance: 66.5617294
Distance: 29.9753094
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 7.46893883
Y: -6.24155045
Z: 1.15941584e-05
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: 1.25979626
Pitch: 0.844796181
Target Frame: world
Value: XYOrbit (rviz)
Yaw: 6.12041521
Yaw: 4.41724014
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......
......@@ -71,6 +71,7 @@ class localisation_node {
std::string mclTF;
string localisation_type_name="";
string dataset="";
std::string initial_pose_topic;
Eigen::Affine3d initial_pose;
Eigen::Affine3d tOld;
......@@ -94,8 +95,10 @@ class localisation_node {
std::ofstream res;
std::string resF_name;
std::string laser_link_id;
std::string pose_estimate_topic;
bool initialized;
double time_0;
std::string undistorted_cloud_topic;
velodyne_rawdata::RawData dataParser;
double min_range;
......@@ -277,8 +280,10 @@ class localisation_node {
Eigen::Affine3d Tcorr=pose_*Todom.inverse();
tf::poseEigenToTF(Tcorr, Tcorr_tf);
trans_pub.sendTransform(tf::StampedTransform( Tcorr_tf, ts, rootTF, odomTF) );
tf::poseEigenToTF(Tsens, sensor_tf);
trans_pub.sendTransform( tf::StampedTransform( sensor_tf, ts, baseTF, laser_link_id) );
if(publish_sensor_link){
tf::poseEigenToTF(Tsens, sensor_tf);
trans_pub.sendTransform( tf::StampedTransform( sensor_tf, ts, baseTF, laser_link_id) );
}
cloud_localized.header.frame_id=laser_link_id;
pcl_conversions::toPCL(ts, cloud_localized.header.stamp);
cloud_pub.publish(cloud_localized);
......@@ -294,6 +299,7 @@ public:
param.param<std::string>("points_topic", points_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);
......@@ -304,7 +310,8 @@ public:
param.param<std::string>("base_tf", baseTF, "/base_link");
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, true);
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.);
......@@ -325,6 +332,7 @@ public:
param.param<double>("var_x", var_x, 0.07);
param.param<double>("var_y", var_y, 0.07);
param.param<double>("var_th", var_th, 0.035);
param.param<string>("initial_pose_topic", initial_pose_topic, "initialpose");
param.param<double>("r_var_x", r_var_x, 1.0);
param.param<double>("r_var_y", r_var_y, 1.0);
......@@ -389,8 +397,8 @@ public:
initial_pose_set=true;
}
mclPosePub=nh.advertise<nav_msgs::Odometry>("ndt_mcl_pose", 20);
cloud_pub=nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("cloud_localized", 20);
mclPosePub=nh.advertise<nav_msgs::Odometry>(pose_estimate_topic, 20);
cloud_pub=nh.advertise<pcl::PointCloud<pcl::PointXYZ>>(undistorted_cloud_topic, 20);
ros::spin();
}
};
......
<launch>
<arg name="robot_prefix" default="/robot5"/>
<node pkg="rosbag" type="play" name="player" output="screen" args="--clock $(find ndt_offline)/bag/volvo_2017_12_01/mapping/S7.bag -r10"/>
<node pkg="velodyne_pointcloud" type="cloud_node" name="convert_veloscans">
<param name="calibration" value="$(find ndt_offline)/config/64e_utexas.yaml" />
</node>
<node pkg="ndt_calibration" type="ndt_calib_online_node" name="ndt_calib_online_node" output="screen">
<param name="points_topic" value="/velodyne_points" />
<param name="points_topic" value="$(arg robot_prefix)/velodyne_points" />
<param name="laser_topic" value="" />
<param name="ct" value="false"/>
<param name="resolution" value="0.8"/>
<param name="min_nb_calibration_pairs" value="10" />
<param name="max_nb_calibration_pairs" value="50"/>
<param name="min_laser_range" value="4"/>
<param name="sensor_pose_x" value="0"/>
<param name="sensor_pose_y" value="0.024"/>
<param name="resolution" value="0.65"/>
<param name="min_nb_calibration_pairs" value="3" />
<param name="max_nb_calibration_pairs" value="15"/>
<param name="min_laser_range" value="2"/>
<param name="sensor_pose_x" value="0.40"/>
<param name="sensor_pose_y" value="0.00"/>
<param name="sensor_pose_z" value="0.95"/>
<param name="sensor_pose_r" value="0.00"/>
<param name="sensor_pose_p" value="0.00"/>
......
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