Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix TF and odometry calculation #331

Merged
merged 6 commits into from Nov 16, 2018
Merged

Fix TF and odometry calculation #331

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e1254c0
Fixed an issue with TF
Myzhar Nov 14, 2018
595a8b5
Fixed an issue with TF
Myzhar Nov 14, 2018
f8697f2
Debug info to check the transform chain map -> odom -> base
Myzhar Nov 14, 2018
11c28b9
Fixed TF issue
Myzhar Nov 15, 2018
5861dcd
Code cleaning
Myzhar Nov 15, 2018
4166bd2
Masked debug TF checking code
Myzhar Nov 16, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
129 changes: 61 additions & 68 deletions zed_display_rviz/rviz/zedm.rviz
View file
Open in desktop
Expand Up @@ -7,7 +7,7 @@ Panels:
- /Global Options1
- /RobotModel1/Links1
- /TF1/Frames1
- /Video1
- /Video1/Camera view1
- /Video1/Camera view1/Visibility1
- /Video1/Camera view1/Visibility1/Video1
- /Video1/Camera view1/Visibility1/Depth1
Expand All @@ -16,9 +16,6 @@ Panels:
- /Pose1/Odometry1/Covariance1
- /Pose1/Odometry1/Covariance1/Position1
- /Pose1/Odometry1/Covariance1/Orientation1
- /Pose1/PoseWithCovariance1/Covariance1
- /Pose1/PoseWithCovariance1/Covariance1/Position1
- /Pose1/PoseWithCovariance1/Covariance1/Orientation1
Splitter Ratio: 0.5
Tree Height: 421
- Class: rviz/Selection
Expand Down Expand Up @@ -107,8 +104,6 @@ Visualization Manager:
Frame Timeout: 5
Frames:
All Enabled: false
imu_link:
Value: true
map:
Value: true
odom:
Expand All @@ -132,8 +127,6 @@ Visualization Manager:
map:
odom:
zed_camera_center:
imu_link:
{}
zed_left_camera_frame:
zed_left_camera_optical_frame:
{}
Expand All @@ -159,9 +152,9 @@ Visualization Manager:
Confidence image: true
Confidence map: true
Depth map: true
DepthCloud: true
PointCloud2: false
Value: true
DepthCloud: false
PointCloud2: true
Value: false
Grid: true
Pose:
Imu: true
Expand Down Expand Up @@ -315,29 +308,6 @@ Visualization Manager:
Name: Depth
- Class: rviz/Group
Displays:
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 25; 0
Enabled: true
Head Diameter: 0.0299999993
Head Length: 0.0199999996
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Odometry Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 25; 0
Pose Style: Arrows
Radius: 0.0299999993
Shaft Diameter: 0.00999999978
Shaft Length: 0.0500000007
Topic: /zed/path_odom
Unreliable: true
Value: true
- Angle Tolerance: 0
Class: rviz/Odometry
Covariance:
Expand All @@ -355,7 +325,7 @@ Visualization Manager:
Scale: 10
Value: true
Value: true
Enabled: false
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0
Expand All @@ -371,36 +341,13 @@ Visualization Manager:
Value: Arrow
Topic: /zed/odom
Unreliable: false
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.0299999993
Head Length: 0.0199999996
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Pose Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 0
Pose Style: Arrows
Radius: 0.0299999993
Shaft Diameter: 0.00999999978
Shaft Length: 0.0500000007
Topic: /zed/path_map
Unreliable: true
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.100000001
Class: rviz/Pose
Color: 0; 255; 0
Enabled: false
Enabled: true
Head Length: 0.100000001
Head Radius: 0.0500000007
Name: Pose
Expand All @@ -409,7 +356,7 @@ Visualization Manager:
Shape: Arrow
Topic: /zed/pose
Unreliable: false
Value: false
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.100000001
Expand Down Expand Up @@ -440,6 +387,52 @@ Visualization Manager:
Topic: /zed/pose_with_covariance
Unreliable: false
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 25; 0
Enabled: false
Head Diameter: 0.0299999993
Head Length: 0.0199999996
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Odometry Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 25; 0
Pose Style: Arrows
Radius: 0.0299999993
Shaft Diameter: 0.00999999978
Shaft Length: 0.0500000007
Topic: /zed/path_odom
Unreliable: true
Value: false
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: false
Head Diameter: 0.0299999993
Head Length: 0.0199999996
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Pose Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 0
Pose Style: Arrows
Radius: 0.0299999993
Shaft Diameter: 0.00999999978
Shaft Length: 0.0500000007
Topic: /zed/path_map
Unreliable: true
Value: false
- Acceleration properties:
Acc. vector alpha: 1
Acc. vector color: 255; 0; 0
Expand All @@ -457,11 +450,11 @@ Visualization Manager:
y_scale: 1
z_scale: 1
Class: rviz_imu_plugin/Imu
Enabled: true
Enabled: false
Name: Imu
Topic: /zed/imu/data
Unreliable: false
Value: true
Value: false
fixed_frame_orientation: true
Enabled: true
Name: Pose
Expand Down Expand Up @@ -490,25 +483,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 1.97160459
Distance: 2.38948631
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.00557371555
Y: 0.107659221
Z: 0.396509618
X: 0.404032081
Y: -0.240342453
Z: -0.078290835
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.0153978756
Pitch: 0.234796301
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.37040448
Yaw: 3.08040643
Saved: ~
Window Geometry:
Camera view:
Expand Down
9 changes: 5 additions & 4 deletions zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp
View file
Open in desktop
Expand Up @@ -91,7 +91,7 @@ namespace zed_wrapper {
* \param slPose : latest odom pose from ZED SDK
* \param t : the ros::Time to stamp the image
*/
void publishOdom(tf2::Transform base2odomTransf, sl::Pose& slPose, ros::Time t);
void publishOdom(tf2::Transform odom2baseTransf, sl::Pose& slPose, ros::Time t);

/* \brief Publish the pose of the camera in "Map" frame as a transformation
* \param baseTransform : Transformation representing the camera pose from
Expand Down Expand Up @@ -370,9 +370,10 @@ namespace zed_wrapper {
std::vector<geometry_msgs::PoseStamped> mMapPath;

// TF Transforms
tf2::Transform mOdom2MapTransf;
tf2::Transform mBase2OdomTransf;
tf2::Transform mSensor2BaseTransf;
tf2::Transform mMap2OdomTransf; // Coordinates of the odometry frame in map frame
tf2::Transform mOdom2BaseTransf; // Coordinates of the base in odometry frame
tf2::Transform mMap2BaseTransf; // Coordinates of the base in base frame
tf2::Transform mSensor2BaseTransf; // Coordinates of the base frame in sensor frame

// Zed object
sl::InitParameters mZedParams;
Expand Down