Skip to content

v2.5 merge #236

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

Merged
adujardin merged 57 commits into from
Jul 17, 2018
Merged

v2.5 merge #236

Changes from 1 commit
Commits
Show all changes
57 commits
Select commit Hold shift + click to select a range
fed6f70
Add internal IMU data publisher by ros::Timer
tongtybj Jun 5, 2018
291ccf5
Modification for a correct merge
Myzhar Jun 25, 2018
c218cd2
Merge branch 'tongtybj-imu_pub' into devel
Myzhar Jun 25, 2018
4378c2e
Merge pull request #217 from stereolabs/master
Myzhar Jun 25, 2018
e1c80d9
Merge pull request #218 from stereolabs/master
Myzhar Jun 25, 2018
90fe983
Corrected issue with IMU reference frame
Myzhar Jun 26, 2018
845e96b
Code reformatting
Myzhar Jun 26, 2018
c893e79
Removed unuseful parameters from publisher functions that were using …
Myzhar Jun 26, 2018
1a7857c
Merged PR #189 : Add publisher for disparity
Myzhar Jun 26, 2018
f2bbd93
Added launch example to run ZED as a nodelet with ROS nodelet manager
Myzhar Jun 27, 2018
9571146
Code refactoring
Myzhar Jun 27, 2018
7c45753
Code refactoring
Myzhar Jun 27, 2018
19e1f0c
Added coordinate system conversion to ROS according to ZED SDK version
Myzhar Jun 27, 2018
633b848
Improved performances using OpenMP
Myzhar Jun 28, 2018
4870071
Modify resolution by launch file
Myzhar Jun 28, 2018
1157cfe
Coordinate changes for Z_UP-X_FWD removed
Myzhar Jun 28, 2018
68d472d
Added ROS service to request resetTracking to ZED SDK
Myzhar Jun 29, 2018
5d0d7fe
Reorganized ZED TF Tree according to ROS conventions
Myzhar Jun 29, 2018
027aabc
Minor changes to file names and code order
Myzhar Jul 2, 2018
f44f0e3
Fixed compilation error with SDK minor than v2.5.x
Myzhar Jul 2, 2018
7f939a5
Fixed issue #212 : zed-wrapper-node running on TX2 freezes when no
Myzhar Jul 2, 2018
a5eec02
Publishing odometry when depth_map topic is subscribed (same as for
Myzhar Jul 2, 2018
2563432
Added publishers for Confidence Image (8 bit) and Confidence Map (float)
Myzhar Jul 2, 2018
d9e88e9
Exposed enable_pose_smoothing and enable_spatial_memory params for
Myzhar Jul 3, 2018
8fa4b52
Exposed "initial_pose" to initialize tracking position
Myzhar Jul 3, 2018
f664f87
Modified "reset_tracking" to reset to "initial_pose" from Param Server
Myzhar Jul 3, 2018
452aaa8
Added service to set pose
Myzhar Jul 4, 2018
29c681b
Fixed issue with "Camera info" when setting new "mat_resize_factor"
Myzhar Jul 5, 2018
9f44001
Added missing set_pose.srv
Myzhar Jul 5, 2018
8c279bd
Removed odometry and replaced with "map pose"
Myzhar Jul 5, 2018
ae74d6c
Fixes the "unchecking/checking" depth cloud problem
Myzhar Jul 5, 2018
9178296
Added tool function slTime2Ros to fix timestamp
Myzhar Jul 5, 2018
195fd68
Coordinate changing code cleaned
Myzhar Jul 6, 2018
95bdebd
Added "imu/data_raw" topic according to ROS REP145
Myzhar Jul 6, 2018
f0be773
Update to publish IMU TF and topics at about 500 Hz
Myzhar Jul 6, 2018
d7ccb07
Added IMU R,t transform to zed_camera_center [TO BE CHECKED]
Myzhar Jul 6, 2018
a6f4136
Fixed issue with IMU link timestamp
Myzhar Jul 9, 2018
c226489
Added back odometry frame (together with map)
Myzhar Jul 9, 2018
b9b9659
Fixed IMU reference frame
Myzhar Jul 10, 2018
5087218
Fixed odometry transformation. Delta movements that are in camera frame
Myzhar Jul 11, 2018
ffc7ad3
Fixed IMU frame publishing
Myzhar Jul 12, 2018
22014ee
Added "verbose" parameter as for zendesk request
Myzhar Jul 12, 2018
668911d
Removed unuseful `include`
Myzhar Jul 13, 2018
e627f2c
Uniformed member variable names to camel case
Myzhar Jul 13, 2018
9c5c346
Modified logger level automatically to "Debug" if node is compiled with
Myzhar Jul 13, 2018
832be65
Added debug info about working thread duration in case it does not
Myzhar Jul 13, 2018
0c38c3a
Exposed "frame_rate" to "zed.launch" file
Myzhar Jul 13, 2018
6f04504
Added option "init_odom_with_imu" to initialize odometry with first pose
Myzhar Jul 16, 2018
8d7220b
Added check on the couple "resolution/framerate".
Myzhar Jul 16, 2018
544f74d
Updated all launch files to latest modifications
Myzhar Jul 16, 2018
3a98446
Fixed bug when the couple "resolution/framerate" was valid
Myzhar Jul 16, 2018
edc4e0f
Added service to reset odometry and clear drift errors.
Myzhar Jul 16, 2018
88bffb2
Exposed the SVO and Odometry Frame parameters to "zed.launch"
Myzhar Jul 16, 2018
7f8d8cf
Reduced the number of warning messages in the case that elaboration time
Myzhar Jul 16, 2018
775a963
Fixed "SIGTERM if no ZED was open" using the Serial Number
Myzhar Jul 16, 2018
1f0d41d
Code cleaning
Myzhar Jul 16, 2018
9c7c434
Fix for IMU TF not propagating in Rviz
Myzhar Jul 16, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Fixed compilation error with SDK minor than v2.5.x 
 * Added MACROS for coordinate frames for backward compatibility
  • Loading branch information
@Myzhar
Myzhar committed Jul 2, 2018
commit f44f0e3121185aa3933770aebcad59cf76be877c
31 changes: 19 additions & 12 deletions src/nodelet/src/zed_wrapper_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,13 @@
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// >>>>> Backward compatibility
#define COORDINATE_SYSTEM_IMAGE static_cast<sl::COORDINATE_SYSTEM>(0)
#define COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP static_cast<sl::COORDINATE_SYSTEM>(3)
#define COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD static_cast<sl::COORDINATE_SYSTEM>(5)
// <<<<< Backward compatibility


using namespace std;

namespace zed_wrapper {
Expand Down Expand Up @@ -238,14 +245,14 @@ namespace zed_wrapper {

if( ver_major<2 ) {
NODELET_WARN_STREAM( "Please consider to upgrade to latest SDK version to get better performances");
param.coordinate_system = sl::COORDINATE_SYSTEM_IMAGE;
param.coordinate_system = COORDINATE_SYSTEM_IMAGE;
NODELET_INFO_STREAM("Camera coordinate system set to COORDINATE_SYSTEM_IMAGE");
} else if( ver_major==2 && ver_minor<5) {
NODELET_WARN_STREAM( "Please consider to upgrade to latest SDK version to get latest features");
param.coordinate_system = sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP;
param.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP;
NODELET_INFO_STREAM("Camera coordinate system set to COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP");
} else {
param.coordinate_system = sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD;
param.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD;
NODELET_INFO_STREAM("Camera coordinate system set to COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD");
}

Expand Down Expand Up @@ -480,7 +487,7 @@ namespace zed_wrapper {

sl::Vector4<float>* cpu_cloud = cloud.getPtr<sl::float4>();

if( param.coordinate_system == sl::COORDINATE_SYSTEM_IMAGE ) {
if( param.coordinate_system == COORDINATE_SYSTEM_IMAGE ) {
#pragma omp parallel for
for (int i = 0; i < size; i++) {
// COORDINATE_SYSTEM_IMAGE
Expand All @@ -489,7 +496,7 @@ namespace zed_wrapper {
point_cloud.points[i].z = -cpu_cloud[i][1];
point_cloud.points[i].rgb = cpu_cloud[i][3];
}
} else if( param.coordinate_system == sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ) {
} else if( param.coordinate_system == COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ) {
#pragma omp parallel for
for (int i = 0; i < size; i++) {
// COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP
Expand All @@ -498,7 +505,7 @@ namespace zed_wrapper {
point_cloud.points[i].z = cpu_cloud[i][2];
point_cloud.points[i].rgb = cpu_cloud[i][3];
}
} else if( param.coordinate_system == sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ) {
} else if( param.coordinate_system == COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ) {
#pragma omp parallel for
for (int i = 0; i < size; i++) {
// COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD
Expand Down Expand Up @@ -646,7 +653,7 @@ namespace zed_wrapper {
imu_msg.header.stamp = ros::Time().fromNSec(imu_data.timestamp);
imu_msg.header.frame_id = imu_frame_id;

if( param.coordinate_system == sl::COORDINATE_SYSTEM_IMAGE ) {
if( param.coordinate_system == COORDINATE_SYSTEM_IMAGE ) {
// COORDINATE_SYSTEM_IMAGE
imu_msg.orientation.x = imu_data.getOrientation()[2];
imu_msg.orientation.y = -imu_data.getOrientation()[0];
Expand Down Expand Up @@ -675,7 +682,7 @@ namespace zed_wrapper {
imu_msg.angular_velocity_covariance[i*3+1] = imu_data.angular_velocity_convariance.r[i*3+0];
imu_msg.angular_velocity_covariance[i*3+2] = imu_data.angular_velocity_convariance.r[i*3+1];
}
} else if( param.coordinate_system == sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ) {
} else if( param.coordinate_system == COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ) {
// COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP
imu_msg.orientation.x = imu_data.getOrientation()[1];
imu_msg.orientation.y = -imu_data.getOrientation()[0];
Expand Down Expand Up @@ -704,7 +711,7 @@ namespace zed_wrapper {
imu_msg.angular_velocity_covariance[i*3+1] = imu_data.angular_velocity_convariance.r[i*3+0];
imu_msg.angular_velocity_covariance[i*3+2] = imu_data.angular_velocity_convariance.r[i*3+2];
}
} else if( param.coordinate_system == sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ) {
} else if( param.coordinate_system == COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ) {
// COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD
imu_msg.orientation.x = imu_data.getOrientation()[0];
imu_msg.orientation.y = imu_data.getOrientation()[1];
Expand Down Expand Up @@ -976,7 +983,7 @@ namespace zed_wrapper {
sl::Translation translation = pose.getTranslation();
sl::Orientation quat = pose.getOrientation();

if( param.coordinate_system == sl::COORDINATE_SYSTEM_IMAGE ) {
if( param.coordinate_system == COORDINATE_SYSTEM_IMAGE ) {
// COORDINATE_SYSTEM_IMAGE
c2s.translation.x = translation(2);
c2s.translation.y = -translation(0);
Expand All @@ -986,7 +993,7 @@ namespace zed_wrapper {
c2s.rotation.y = -quat(0);
c2s.rotation.z = -quat(1);
c2s.rotation.w = quat(3);
} else if( param.coordinate_system == sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ) {
} else if( param.coordinate_system == COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP ) {
// COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP
c2s.translation.x = translation(1);
c2s.translation.y = -translation(0);
Expand All @@ -996,7 +1003,7 @@ namespace zed_wrapper {
c2s.rotation.y = -quat(0);
c2s.rotation.z = quat(2);
c2s.rotation.w = quat(3);
} else if( param.coordinate_system == sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ) {
} else if( param.coordinate_system == COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ) {
// COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD
c2s.translation.x = translation(0);
c2s.translation.y = translation(1);
Expand Down