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

updated transforms

parent 371d1f94
......@@ -11,11 +11,9 @@ Panels:
- /Odometry1/Shape1
- /Odometry2/Shape1
- /Odometry3/Shape1
- /PoseArray1
- /Odometry4/Shape1
- /PointCloud22/Autocompute Value Bounds1
- /NDT1
- /Map1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 815
- Class: rviz/Selection
......@@ -37,7 +35,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
......@@ -68,8 +66,64 @@ Visualization Manager:
Value: true
map_velodyne:
Value: true
robot4/base_footprint:
Value: true
robot4/base_fork:
Value: true
robot4/base_link:
Value: true
robot4/base_link_ground_truth:
Value: true
robot4/camera_depth_frame:
Value: true
robot4/camera_depth_optical_frame:
Value: true
robot4/camera_link:
Value: true
robot4/camera_rgb_frame:
Value: true
robot4/camera_rgb_optical_frame:
Value: true
robot4/fixed_left_wheel_link:
Value: true
robot4/fixed_right_wheel_link:
Value: true
robot4/kinect2_depth_optical_frame:
Value: true
robot4/kinect2_ir_optical_frame:
Value: true
robot4/kinect2_link:
Value: true
robot4/kinect2_rgb_optical_frame:
Value: true
robot4/kinect_link:
Value: true
robot4/laser2d_floor:
Value: true
robot4/laser2d_floor_link:
Value: true
robot4/laser2d_top:
Value: true
robot4/laser2d_top_link:
Value: true
robot4/left_fork:
Value: true
robot4/odom:
Value: true
robot4/right_fork:
Value: true
robot4/robot4/velodyne:
Value: true
robot4/robot4/velodyne_base_link:
Value: true
robot4/sd_wheel_link:
Value: true
robot4/steer_link:
Value: true
submap_node0:
Value: true
velodyne:
Value: true
world:
Value: true
Marker Scale: 1
......@@ -83,6 +137,42 @@ Visualization Manager:
{}
map_velodyne:
{}
robot4/odom:
robot4/base_footprint:
robot4/base_link:
robot4/base_fork:
robot4/left_fork:
{}
robot4/right_fork:
{}
robot4/fixed_left_wheel_link:
{}
robot4/fixed_right_wheel_link:
{}
robot4/kinect2_link:
robot4/kinect2_depth_optical_frame:
{}
robot4/kinect2_rgb_optical_frame:
robot4/kinect2_ir_optical_frame:
{}
robot4/kinect_link:
{}
robot4/laser2d_floor:
{}
robot4/laser2d_floor_link:
{}
robot4/laser2d_top:
{}
robot4/laser2d_top_link:
{}
robot4/robot4/velodyne_base_link:
robot4/robot4/velodyne:
{}
robot4/steer_link:
robot4/sd_wheel_link:
{}
velodyne:
{}
submap_node0:
{}
Update Interval: 0
......@@ -95,36 +185,6 @@ Visualization Manager:
{}
Queue Size: 100
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 5
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Flat Squares
Topic: /graph_node/point2_fuser
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Covariance:
......@@ -277,7 +337,7 @@ Visualization Manager:
Topic: /ndt_mcl_pose
Unreliable: false
Value: true
- Alpha: 1
- Alpha: 0.800000012
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 5.9265604
......@@ -351,25 +411,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance: 45.4304314
Distance: 18.1015873
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -5.52330637
Y: -10.7744989
Z: 0
X: -3.50539112
Y: -25.8522511
Z: 5.3598078e-06
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.14520371
Pitch: -0.0452038944
Target Frame: world
Value: XYOrbit (rviz)
Yaw: 5.13040686
Yaw: 0.784042537
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......
......@@ -189,7 +189,7 @@ class localisation_node {
pnts.clear();
}
this->processFrame(cloud,t_cloud_end);
cout<<"Tdiff="<<ros::Time::now()-t0<<endl;
}
void PCCallback(const sensor_msgs::PointCloud2::ConstPtr& msg){
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
......@@ -272,16 +272,15 @@ class localisation_node {
mean_pose_odom.header.stamp = ts;
mclPosePub.publish(mean_pose_odom);
if(visualize){
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) );
cloud_localized.header.frame_id=laser_link_id;
pcl_conversions::toPCL(ts, cloud_localized.header.stamp);
cloud_pub.publish(cloud_localized);
}
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) );
cloud_localized.header.frame_id=laser_link_id;
pcl_conversions::toPCL(ts, cloud_localized.header.stamp);
cloud_pub.publish(cloud_localized);
cloud.clear();
cloud_localized.clear();
}
......
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