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

Show file tree
Hide file tree
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
Added publishers for Confidence Image (8 bit) and Confidence Map (float)
  • Loading branch information
@Myzhar
Myzhar committed Jul 2, 2018
commit 2563432cadcd73f316d1812a35898118a65cfd08
4 changes: 4 additions & 0 deletions launch/zed_camera.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

<param name="point_cloud_topic" value="point_cloud/cloud_registered" />

<param name="disparity_topic" value="disparity/disparity_image" />
<param name="confidence_img_topic" value="confidence/confidence_image" />
<param name="confidence_map_topic" value="confidence/confidence_map" />

<param name="odometry_topic" value="odom" />

<param name="imu_topic" value="imu/data" />
Expand Down
135 changes: 71 additions & 64 deletions launch/zed_camera_nodelet.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,82 +22,89 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<arg name="zed_id" default="0" />
<!-- If a ZED SN is given, zed_id is ignored and the wrapper will specifically look for the ZED with the corresponding serial number -->
<arg name="serial_number" default="0" />
<arg name="resolution" default="2" />

<!-- GPU ID-->
<arg name="gpu_id" default="-1" />

<!-- Definition coordinate frames -->
<arg name="publish_tf" default="true" />
<arg name="odometry_frame" default="odom" />
<arg name="base_frame" default="zed_center" />
<arg name="camera_frame" default="zed_left_camera" />
<arg name="depth_frame" default="zed_depth_camera" />
<arg name="disparity_frame" default="zed_depth_camera" />
<arg name="imu_frame" default="imu_link" />
<arg name="odometry_frame" default="odom" />
<arg name="base_frame" default="zed_center" />
<arg name="left_camera_frame" default="zed_left_camera_frame" />
<arg name="left_camera_optical_frame" default="zed_left_camera_optical_frame" />
<arg name="right_camera_frame" default="zed_right_camera_frame" />
<arg name="right_camera_optical_frame" default="zed_right_camera_optical_frame" />
<arg name="imu_frame" default="imu_link" />

<!-- Publish urdf zed -->
<arg name="publish_urdf" default="true" />

<node pkg="nodelet" type="nodelet" name="ZED_nodelet" args="load zed_wrapper/ZEDWrapperNodelet nodelet_manager" output="screen">

<param name="camera_model" value="$(arg camera_model)" />

<!-- publish odometry frame -->
<param name="publish_tf" value="$(arg publish_tf)" />

<!-- Configuration frame camera -->
<param name="odometry_frame" value="$(arg odometry_frame)" />
<param name="base_frame" value="$(arg base_frame)" />
<param name="camera_frame" value="$(arg camera_frame)" />
<param name="depth_frame" value="$(arg depth_frame)" />
<param name="disparity_frame" value="$(arg disparity_frame)" />

<!-- SVO file path -->
<param name="svo_filepath" value="$(arg svo_file)" />

<!-- ZED parameters -->
<param name="zed_id" value="$(arg zed_id)" />
<param name="serial_number" value="$(arg serial_number)" />

<param name="resolution" value="2" />
<param name="quality" value="1" />
<param name="sensing_mode" value="0" />
<param name="frame_rate" value="60" />
<param name="odometry_db" value="" />
<param name="openni_depth_mode" value="0" />
<param name="gpu_id" value="$(arg gpu_id)" />
<param name="confidence" value="100" />
<param name="gain" value="100" />
<param name="exposure" value="100" />
<param name="auto_exposure" value="true" />
<param name="depth_stabilization" value="1" />

<!-- ROS topic names -->
<param name="rgb_topic" value="rgb/image_rect_color" />
<param name="rgb_raw_topic" value="rgb/image_raw_color" />
<param name="rgb_cam_info_topic" value="rgb/camera_info" />
<param name="rgb_cam_info_raw_topic" value="rgb/camera_info_raw" />

<param name="left_topic" value="left/image_rect_color" />
<param name="left_raw_topic" value="left/image_raw_color" />
<param name="left_cam_info_topic" value="left/camera_info" />
<param name="left_cam_info_raw_topic" value="left/camera_info_raw" />

<param name="right_topic" value="right/image_rect_color" />
<param name="right_raw_topic" value="right/image_raw_color" />
<param name="right_cam_info_topic" value="right/camera_info" />
<param name="right_cam_info_raw_topic" value="right/camera_info_raw" />

<param name="depth_topic" value="depth/depth_registered" />
<param name="depth_cam_info_topic" value="depth/camera_info" />

<param name="point_cloud_topic" value="point_cloud/cloud_registered" />

<param name="odometry_topic" value="odom" />

<param name="imu_topic" value="imu/data" />

<param name="imu_pub_rate" value="100.0" />
<param name="camera_model" value="$(arg camera_model)" />

<!-- publish odometry frame -->
<param name="publish_tf" value="$(arg publish_tf)" />

<!-- Configuration frame camera -->
<param name="odometry_frame" value="$(arg odometry_frame)" />
<param name="base_frame" value="$(arg base_frame)" />
<param name="left_camera_frame" value="$(arg left_camera_frame)" />
<param name="left_camera_optical_frame" value="$(arg left_camera_optical_frame)" />
<param name="right_camera_frame" value="$(arg right_camera_frame)" />
<param name="right_camera_optical_frame" value="$(arg right_camera_optical_frame)" />

<!-- SVO file path -->
<param name="svo_filepath" value="$(arg svo_file)" />

<!-- ZED parameters -->
<param name="zed_id" value="$(arg zed_id)" />
<param name="serial_number" value="$(arg serial_number)" />

<param name="resolution" value="$(arg resolution)" />
<param name="quality" value="1" />
<param name="sensing_mode" value="0" />
<param name="frame_rate" value="60" />
<param name="odometry_db" value="" />
<param name="openni_depth_mode" value="0" />
<param name="gpu_id" value="$(arg gpu_id)" />
<param name="confidence" value="100" />
<param name="gain" value="100" />
<param name="exposure" value="100" />
<param name="auto_exposure" value="true" />
<param name="depth_stabilization" value="1" />

<!-- ROS topic names -->
<param name="rgb_topic" value="rgb/image_rect_color" />
<param name="rgb_raw_topic" value="rgb/image_raw_color" />
<param name="rgb_cam_info_topic" value="rgb/camera_info" />
<param name="rgb_cam_info_raw_topic" value="rgb/camera_info_raw" />

<param name="left_topic" value="left/image_rect_color" />
<param name="left_raw_topic" value="left/image_raw_color" />
<param name="left_cam_info_topic" value="left/camera_info" />
<param name="left_cam_info_raw_topic" value="left/camera_info_raw" />

<param name="right_topic" value="right/image_rect_color" />
<param name="right_raw_topic" value="right/image_raw_color" />
<param name="right_cam_info_topic" value="right/camera_info" />
<param name="right_cam_info_raw_topic" value="right/camera_info_raw" />

<param name="depth_topic" value="depth/depth_registered" />
<param name="depth_cam_info_topic" value="depth/camera_info" />

<param name="point_cloud_topic" value="point_cloud/cloud_registered" />

<param name="disparity_topic" value="disparity/disparity_image" />
<param name="confidence_img_topic" value="confidence/confidence_image" />
<param name="confidence_map_topic" value="confidence/confidence_map" />

<param name="odometry_topic" value="odom" />

<param name="imu_topic" value="imu/data" />

<param name="imu_pub_rate" value="100.0" />

</node>

Expand Down
11 changes: 11 additions & 0 deletions src/nodelet/include/zed_wrapper_nodelet.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,12 @@ namespace zed_wrapper {
*/
void publishDepth(cv::Mat depth, ros::Time t);

/* \brief Publish a cv::Mat confidence image with a ros Publisher
* \param conf : the confidence image to publish
* \param t : the ros::Time to stamp the depth image
*/
void publishConf(cv::Mat conf, ros::Time t);

/* \brief Publish a pointCloud with a ros Publisher
* \param width : the width of the point cloud
* \param height : the height of the point cloud
Expand Down Expand Up @@ -167,6 +173,8 @@ namespace zed_wrapper {
image_transport::Publisher pub_right;
image_transport::Publisher pub_raw_right;
image_transport::Publisher pub_depth;
image_transport::Publisher pub_conf_img;
ros::Publisher pub_conf_map;
ros::Publisher pub_disparity;
ros::Publisher pub_cloud;
ros::Publisher pub_rgb_cam_info;
Expand Down Expand Up @@ -195,6 +203,9 @@ namespace zed_wrapper {
std::string disparity_frame_id;
std::string disparity_opt_frame_id;

std::string confidence_frame_id;
std::string confidence_opt_frame_id;

std::string cloud_frame_id;

std::string odometry_frame_id;
Expand Down
49 changes: 41 additions & 8 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 @@ -56,7 +56,6 @@
#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 @@ -105,7 +104,10 @@ namespace zed_wrapper {
rgb_opt_frame_id = cloud_frame_id;

disparity_frame_id = depth_frame_id;
disparity_opt_frame_id = depth_opt_frame_id;
disparity_opt_frame_id = depth_opt_frame_id;

confidence_frame_id = depth_frame_id;
confidence_opt_frame_id = depth_opt_frame_id;

// Get parameters from launch file
nh_ns.getParam("resolution", resolution);
Expand Down Expand Up @@ -177,6 +179,9 @@ namespace zed_wrapper {

string point_cloud_topic = "point_cloud/cloud_registered";

string conf_img_topic = "confidence/confidence_image";
string conf_map_topic = "confidence/confidence_map";

string odometry_topic = "odom";
string imu_topic = "imu";

Expand All @@ -200,6 +205,9 @@ namespace zed_wrapper {

nh_ns.getParam("disparity_topic", disparity_topic);

nh_ns.getParam("confidence_img_topic", conf_img_topic);
nh_ns.getParam("confidence_map_topic", conf_map_topic);

nh_ns.getParam("point_cloud_topic", point_cloud_topic);

nh_ns.getParam("odometry_topic", odometry_topic);
Expand Down Expand Up @@ -324,7 +332,14 @@ namespace zed_wrapper {
pub_raw_right = it_zed.advertise(right_raw_topic, 1); //right raw
NODELET_INFO_STREAM("Advertized on topic " << right_raw_topic);
pub_depth = it_zed.advertise(depth_topic, 1); //depth
NODELET_INFO_STREAM("Advertized on topic " << depth_topic);
NODELET_INFO_STREAM("Advertized on topic " << depth_topic);
pub_conf_img = it_zed.advertise(conf_img_topic, 1); //confidence image
NODELET_INFO_STREAM("Advertized on topic " << conf_img_topic);

// Confidence Map publisher
pub_conf_map = nh.advertise<sensor_msgs::Image>(conf_map_topic, 1); //confidence map
NODELET_INFO_STREAM("Advertized on topic " << conf_map_topic);


// Disparity publisher
pub_disparity = nh.advertise<stereo_msgs::DisparityImage>(disparity_topic, 1);
Expand Down Expand Up @@ -759,6 +774,8 @@ namespace zed_wrapper {
cv::Size cvSize(width, height);
cv::Mat leftImRGB(cvSize, CV_8UC3);
cv::Mat rightImRGB(cvSize, CV_8UC3);
cv::Mat confImRGB(cvSize, CV_8UC3);
cv::Mat confMapFloat(cvSize, CV_32FC1);

// Create and fill the camera information messages
sensor_msgs::CameraInfoPtr rgb_cam_info_msg(new sensor_msgs::CameraInfo());
Expand All @@ -783,7 +800,7 @@ namespace zed_wrapper {
trackParams.area_file_path = odometry_DB.c_str();


sl::Mat leftZEDMat, rightZEDMat, depthZEDMat, disparityZEDMat;
sl::Mat leftZEDMat, rightZEDMat, depthZEDMat, disparityZEDMat, confImgZEDMat, confMapZEDMat;
// Main loop
while (nh_ns.ok()) {
// Check for subscribers
Expand All @@ -797,8 +814,10 @@ namespace zed_wrapper {
int disparity_SubNumber = pub_disparity.getNumSubscribers();
int cloud_SubNumber = pub_cloud.getNumSubscribers();
int odom_SubNumber = pub_odom.getNumSubscribers();
int conf_img_SubNumber = pub_conf_img.getNumSubscribers();
int conf_map_SubNumber = pub_conf_map.getNumSubscribers();
bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + left_raw_SubNumber + right_SubNumber +
right_raw_SubNumber + depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber) > 0;
right_raw_SubNumber + depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber + conf_img_SubNumber + conf_map_SubNumber) > 0;

runParams.enable_point_cloud = false;
if (cloud_SubNumber > 0)
Expand All @@ -816,7 +835,7 @@ namespace zed_wrapper {
zed.disableTracking();
tracking_activated = false;
}
computeDepth = (depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
computeDepth = (depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber + conf_img_SubNumber + conf_map_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
ros::Time t = ros::Time::now(); // Get current time

grabbing = true;
Expand Down Expand Up @@ -940,10 +959,10 @@ namespace zed_wrapper {
cv::cvtColor(sl_tools::toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB);
publishCamInfo(right_cam_info_raw_msg, pub_right_cam_info_raw, t);
publishImage(rightImRGB, pub_raw_right, right_cam_opt_frame_id, t);
}
}

// Publish the depth image if someone has subscribed to
if (depth_SubNumber > 0) {
if (depth_SubNumber > 0 || disparity_SubNumber > 0) {
zed.retrieveMeasure(depthZEDMat, sl::MEASURE_DEPTH);
publishCamInfo(depth_cam_info_msg, pub_depth_cam_info, t);
publishDepth(sl_tools::toCVMat(depthZEDMat), t); // in meters
Expand All @@ -957,6 +976,20 @@ namespace zed_wrapper {
publishDisparity(disparity, t);
}

// Publish the confidence image if someone has subscribed to
if (conf_img_SubNumber > 0) {
zed.retrieveImage(confImgZEDMat, sl::VIEW_CONFIDENCE);
cv::cvtColor(sl_tools::toCVMat(confImgZEDMat), confImRGB, CV_RGBA2RGB);
publishImage(confImRGB, pub_conf_img, confidence_opt_frame_id, t);
}

// Publish the confidence map if someone has subscribed to
if (conf_map_SubNumber > 0) {
zed.retrieveMeasure(confMapZEDMat, sl::MEASURE_CONFIDENCE);
confMapFloat = sl_tools::toCVMat(confMapZEDMat);
pub_conf_map.publish(imageToROSmsg(confMapFloat, sensor_msgs::image_encodings::TYPE_32FC1, confidence_opt_frame_id, t));
}

// Publish the point cloud if someone has subscribed to
if (cloud_SubNumber > 0) {
// Run the point cloud conversion asynchronously to avoid slowing down all the program
Expand Down