diff --git a/README.md b/README.md index ef9653e5..ff69d53c 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,11 @@ +![](./images/Picto+STEREOLABS_Black.png) + # Stereolabs ZED Camera - ROS Integration This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, pose information and supports the use of multiple ZED cameras. +[More information](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) + ## Getting started - First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com/developers/) @@ -25,7 +29,6 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package - rosconsole - sensor_msgs - stereo_msgs - - opencv3 - image_transport - dynamic_reconfigure - urdf @@ -57,12 +60,17 @@ To launch the wrapper without Rviz, use: $ roslaunch zed_wrapper zed.launch serial_number:=1010 #replace 1010 with the actual SN -### Modules +### Examples + +Alongside the wrapper itself and the Rviz display, a few examples are provided to interface the ZED with other ROS packages : -Alongside the wrapper itself and the Rviz display, a few other modules are provided to interface the ZED with other ROS packages : +- [RTAB-Map](http://introlab.github.io/rtabmap/) : See [zed_rtabmap_example](./examples/zed_rtabmap_example) +- ROS Nodelet, `depthimage_to_laserscan` : See [zed_nodelet_example](./examples/zed_nodelet_example) -- [RTAB-Map](http://introlab.github.io/rtabmap/) : See [zed_rtabmap_example](./zed_rtabmap_example) -- ROS Nodelet, `depthimage_to_laserscan` : See [zed_nodelet_example](./zed_nodelet_example) +### Tutorials +A few tutorials are provided to understand how to use the ZED node in the ROS environment : -[More](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) +- Video subscribing : See [zed_video_sub_tutorial](./tutorials/zed_video_sub_tutorial) +- Depth subscribing : See [zed_depth_sub_tutorial](./tutorials/zed_depth_sub_tutorial) +- Positional Tracking subscribing : See [zed_tracking_sub_tutorial](./tutorials/zed_tracking_sub_tutorial) diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 00000000..993d0272 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,6 @@ +# Examples +How to use the ZED ROS node with other ROS packages + +* `zed_nodelet_example`: shows how to use the nodelet intraprocess communication to generate a virtual laser scan using the [depthimage_to_laserscan](http://wiki.ros.org/depthimage_to_laserscan) package + +* `zed_rtabmap_example`: shows how to use the ZED with the RTABMap package to generate a 3D map with the [rtabmap_ros](http://wiki.ros.org/rtabmap_ros) package diff --git a/zed_nodelet_example/CMakeLists.txt b/examples/zed_nodelet_example/CMakeLists.txt similarity index 100% rename from zed_nodelet_example/CMakeLists.txt rename to examples/zed_nodelet_example/CMakeLists.txt diff --git a/examples/zed_nodelet_example/README.md b/examples/zed_nodelet_example/README.md new file mode 100644 index 00000000..70e83c2b --- /dev/null +++ b/examples/zed_nodelet_example/README.md @@ -0,0 +1,69 @@ +# Stereolabs ZED Camera - ROS Nodelet example + +`zed_nodelet_example` is a ROS package to illustrate how to load the `ZEDWrapperNodelet` with an external nodelet manager and use the intraprocess communication to generate a virtual laser scan thanks to the nodelet `depthimage_to_laserscan` + +## Run the program + +To launch the wrapper nodelet along with the `depthimage_to_laserscan` nodelet, open a terminal and launch: + +`$ roslaunch zed_nodelet_example zed_nodelet_laserscan.launch` + +**Note**: Remember to change the parameter `camera_model` to `0` if you are using a **ZED** or to `1` if you are using a **ZED Mini** + +## Visualization +To visualize the result of the process open Rviz, add a `LaserScan` visualization and set `/zed/scan` as `topic` parameter + +Virtual 2D laser scan rendered in Rviz. You can see the projection of the virtual laser scan on the RGB image on the left +![Virtual laser scan rendered in Rviz](images/laserscan.png) + +Virtual 2D laser scan rendered in Rviz over the 3D depth cloud. You can see the projection of the virtual laser scan on the RGB image on the left +![Virtual laser scan rendered in Rviz on the Depthcloud](images/laserscan-depthcloud.png) + +## The launch file explained +The launch file executes three main operations: + +1. Runs the Nodelet manager +2. Load the ZED nodelet +3. Load the `depthimage_to_laserscan` nodelet + +To run the Nodelet Manager we use the following line instruction: + +`````` + +the Nodelet Manager is the process that loads the ZED and the depthimage_to_laserscan nodelets and that allows them to us intra-process communication to pass elaboration data. + +The "variable" `nodelet_manager_name` is defined here: + +`````` + +The ZED nodelet is loaded using its own nodelet launch file: + +`````` + +called passing the "variable" `nodelet_manager_name` as parameter: + +`````` + +The `DepthImageToLaserScanNodelet` nodelet is loaded with the following commands: +``` + + + + + + +``` + +it is really important to notice these two lines: + +`````` + +`````` + +The first line tells to the `depthimage_to_laserscan` nodelet which is the frame name of the virtual scan message. + +The second line tells to the `depthimage_to_laserscan` nodelet which is the depth image topic to use to extract the information to generate the virtual laser scan. + +For the description of the other parameters please refer to the [documentation of the `depthimage_to_laserscan` package](http://wiki.ros.org/depthimage_to_laserscan) + + diff --git a/zed_nodelet_example/images/laserscan-depthcloud.png b/examples/zed_nodelet_example/images/laserscan-depthcloud.png similarity index 100% rename from zed_nodelet_example/images/laserscan-depthcloud.png rename to examples/zed_nodelet_example/images/laserscan-depthcloud.png diff --git a/zed_nodelet_example/images/laserscan.png b/examples/zed_nodelet_example/images/laserscan.png similarity index 100% rename from zed_nodelet_example/images/laserscan.png rename to examples/zed_nodelet_example/images/laserscan.png diff --git a/zed_nodelet_example/launch/zed_nodelet_laserscan.launch b/examples/zed_nodelet_example/launch/zed_nodelet_laserscan.launch similarity index 99% rename from zed_nodelet_example/launch/zed_nodelet_laserscan.launch rename to examples/zed_nodelet_example/launch/zed_nodelet_laserscan.launch index 3fae64c7..e6d715c8 100644 --- a/zed_nodelet_example/launch/zed_nodelet_laserscan.launch +++ b/examples/zed_nodelet_example/launch/zed_nodelet_laserscan.launch @@ -37,7 +37,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + diff --git a/zed_nodelet_example/package.xml b/examples/zed_nodelet_example/package.xml similarity index 83% rename from zed_nodelet_example/package.xml rename to examples/zed_nodelet_example/package.xml index 75908476..f274822a 100644 --- a/zed_nodelet_example/package.xml +++ b/examples/zed_nodelet_example/package.xml @@ -1,11 +1,11 @@ zed_nodelet_example - 1.0.0 + 2.6.0 "zed_nodelet_example" is a ROS package to illustrate how to load the ZEDWrapperNodelet with an external nodelet manager and use the intraprocess communication. - STEREOLABS + STEREOLABS BSD catkin diff --git a/zed_rtabmap_example/CMakeLists.txt b/examples/zed_rtabmap_example/CMakeLists.txt similarity index 100% rename from zed_rtabmap_example/CMakeLists.txt rename to examples/zed_rtabmap_example/CMakeLists.txt diff --git a/examples/zed_rtabmap_example/README.md b/examples/zed_rtabmap_example/README.md new file mode 100644 index 00000000..98483107 --- /dev/null +++ b/examples/zed_rtabmap_example/README.md @@ -0,0 +1,54 @@ +# Stereolabs ZED Camera - RTAB-map example + +This package shows how to use the ZED Wrapper with [RTAB-map](http://introlab.github.io/rtabmap/) + +## Run the program + +To launch the example, open a terminal and launch: + + $ roslaunch zed_rtabmap_example zed_rtabmap.launch + +Example of indoor 3D mapping using RTAB-map and ZED + +![Example of indoor 3D mapping](images/rtab-map.png) + +## The launch file explained + +To correctly use the ZED wrapper with the `rtabmap_ros` node we need to match the following RTABmap parameters: + +- `rgb_topic` -> topic of the color information to be associated to the points of the 3D map +- `depth_topic` -> topic of the depth information +- `camera_info_topic` -> topic of RGB camera parameters used to create the association of each color pixel to the relative 3D point +- `depth_camera_info_topic` -> topic of the depth camera parameter, to convert the 2D depth image to a 3D point cloud +- `frame_id` -> name of the camera frame + +The values associated to the above parameters are the following: + +``` + + + + + +``` + +The corresponding parameters of the ZED node are the following: + +- `rgb_topic` -> `rgb_topic` +- `depth_topic` -> `depth_topic` +- `camera_info_topic` -> `rgb_info_topic` +- `depth_camera_info_topic` -> `depth_cam_info_topic` +- `frame_id` -> `base_frame` + +The corresponding parameters of the `rtabmap_ros` node are the following: + +- `rgb_topic` -> `rgb_topic` +- `depth_topic` -> `depth_topic` +- `camera_info_topic` -> `rgb_info_topic` +- `depth_camera_info_topic` -> `depth_cam_info_topic` +- `camera_frame` -> `frame_id` + + + + + diff --git a/zed_rtabmap_example/images/rtab-map.png b/examples/zed_rtabmap_example/images/rtab-map.png similarity index 100% rename from zed_rtabmap_example/images/rtab-map.png rename to examples/zed_rtabmap_example/images/rtab-map.png diff --git a/zed_rtabmap_example/launch/zed_rtabmap.launch b/examples/zed_rtabmap_example/launch/zed_rtabmap.launch similarity index 92% rename from zed_rtabmap_example/launch/zed_rtabmap.launch rename to examples/zed_rtabmap_example/launch/zed_rtabmap.launch index efc2ad5a..b57ddc51 100644 --- a/zed_rtabmap_example/launch/zed_rtabmap.launch +++ b/examples/zed_rtabmap_example/launch/zed_rtabmap.launch @@ -47,16 +47,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - + + - + - + diff --git a/zed_rtabmap_example/package.xml b/examples/zed_rtabmap_example/package.xml similarity index 82% rename from zed_rtabmap_example/package.xml rename to examples/zed_rtabmap_example/package.xml index 822cb27a..6e552beb 100644 --- a/zed_rtabmap_example/package.xml +++ b/examples/zed_rtabmap_example/package.xml @@ -1,11 +1,11 @@ zed_rtabmap_example - 1.0.0 + 2.6.0 "zed_rtabmap_example" is a ROS package to show how to use the ROS wrapper with "RTAB-MAP" - STEREOLABS + STEREOLABS BSD catkin diff --git a/images/Picto+STEREOLABS_Black.png b/images/Picto+STEREOLABS_Black.png new file mode 100644 index 00000000..5dcb9be4 Binary files /dev/null and b/images/Picto+STEREOLABS_Black.png differ diff --git a/tutorials/README.md b/tutorials/README.md new file mode 100644 index 00000000..dca74947 --- /dev/null +++ b/tutorials/README.md @@ -0,0 +1,14 @@ +# Tutorials +A series of tutorials are provided to better understand how to use the ZED node in the ROS environment : + +- [Video subscribing](./zed_video_sub_tutorial) : `zed_video_sub_tutorial` - in this tutorial you will learn how to write a simple node that subscribes to messages of type `sensor_msgs/Image` to retrieve the Left and Right rectified images published by the ZED node + +- [Depth subscribing](./zed_depth_sub_tutorial) : `zed_depth_sub_tutorial` - in this tutorial you will learn how to write a simple node that subscribes to messages of type `sensor_msgs/Image` to retrieve the depth images published by the ZED node and to get the measured distance at the center of the image + +- [Positional tracking subscribing](./zed_tracking_sub_tutorial) : `zed_tracking_sub_tutorial` - in this tutorial you will learn how to write a simple node that subscribes to messages of type `geometry_msgs/PoseStamped` and `nav_msgs/Odometry` to retrieve the position and the orientation of the ZED camera in the map and in the odometry frames. + +For a complete explanation of the tutorials please refer to the Stereolabs ZED online documentation: + +- [Video](https://docs.stereolabs.com/integrations/ros/video/) +- [Depth](https://docs.stereolabs.com/integrations/ros/depth_sensing/) +- [Positional Tracking](https://docs.stereolabs.com/integrations/ros/positional_tracking/) diff --git a/tutorials/images/tutorial_depth.png b/tutorials/images/tutorial_depth.png new file mode 100644 index 00000000..31f84a94 Binary files /dev/null and b/tutorials/images/tutorial_depth.png differ diff --git a/tutorials/images/tutorial_tracking.png b/tutorials/images/tutorial_tracking.png new file mode 100644 index 00000000..16714ac1 Binary files /dev/null and b/tutorials/images/tutorial_tracking.png differ diff --git a/tutorials/images/tutorial_video.png b/tutorials/images/tutorial_video.png new file mode 100644 index 00000000..015a4cc5 Binary files /dev/null and b/tutorials/images/tutorial_video.png differ diff --git a/tutorials/zed_depth_sub_tutorial/CMakeLists.txt b/tutorials/zed_depth_sub_tutorial/CMakeLists.txt new file mode 100644 index 00000000..e4fa05d9 --- /dev/null +++ b/tutorials/zed_depth_sub_tutorial/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zed_depth_sub_tutorial) + +# if CMAKE_BUILD_TYPE is not specified, take 'Release' as default +IF(NOT CMAKE_BUILD_TYPE) + SET(CMAKE_BUILD_TYPE Release) +ENDIF(NOT CMAKE_BUILD_TYPE) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) + +## Build +add_executable(zed_depth_sub src/zed_depth_sub_tutorial.cpp) +target_link_libraries(zed_depth_sub ${catkin_LIBRARIES}) diff --git a/tutorials/zed_depth_sub_tutorial/README.md b/tutorials/zed_depth_sub_tutorial/README.md new file mode 100644 index 00000000..7ca8520a --- /dev/null +++ b/tutorials/zed_depth_sub_tutorial/README.md @@ -0,0 +1,9 @@ +# Depth subscription tutorial + +In this tutorial you will learn how to write a simple node that subscribes to messages of type sensor_msgs/Image to retrieve the depth images published by the ZED node and to get the measured distance at the center of the image + +![](../images/tutorial_depth.png) + +The complete documentation is available on the [Stereolabs website](https://docs.stereolabs.com/integrations/ros/depth_sensing/) + + diff --git a/tutorials/zed_depth_sub_tutorial/package.xml b/tutorials/zed_depth_sub_tutorial/package.xml new file mode 100644 index 00000000..7a54f007 --- /dev/null +++ b/tutorials/zed_depth_sub_tutorial/package.xml @@ -0,0 +1,14 @@ + + zed_depth_sub_tutorial + 2.6.0 + + This package is a tutorial showing how to subscribe to ZED depth streams + + STEREOLABS + BSD + + catkin + + roscpp + sensor_msgs + diff --git a/tutorials/zed_depth_sub_tutorial/src/zed_depth_sub_tutorial.cpp b/tutorials/zed_depth_sub_tutorial/src/zed_depth_sub_tutorial.cpp new file mode 100644 index 00000000..1494fce7 --- /dev/null +++ b/tutorials/zed_depth_sub_tutorial/src/zed_depth_sub_tutorial.cpp @@ -0,0 +1,98 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2018, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + +/** + * This tutorial demonstrates simple receipt of ZED depth messages over the ROS system. + */ + +#include +#include + +/** + * Subscriber callback + */ + +void depthCallback(const sensor_msgs::Image::ConstPtr& msg) { + + // Get a pointer to the depth values casting the data + // pointer to floating point + float* depths = (float*)(&msg->data[0]); + + // Image coordinates of the center pixel + int u = msg->width / 2; + int v = msg->height / 2; + + // Linear index of the center pixel + int centerIdx = u + msg->width * v; + + // Output the measure + ROS_INFO("Center distance : %g m", depths[centerIdx]); +} + +/** + * Node main function + */ +int main(int argc, char** argv) { + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "zed_video_subscriber"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called imageCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + ros::Subscriber subDepth = n.subscribe("/zed/depth/depth_registered", 10, depthCallback); + + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + ros::spin(); + + return 0; +} diff --git a/tutorials/zed_tracking_sub_tutorial/CMakeLists.txt b/tutorials/zed_tracking_sub_tutorial/CMakeLists.txt new file mode 100644 index 00000000..46982403 --- /dev/null +++ b/tutorials/zed_tracking_sub_tutorial/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zed_tracking_sub_tutorial) + +# if CMAKE_BUILD_TYPE is not specified, take 'Release' as default +IF(NOT CMAKE_BUILD_TYPE) + SET(CMAKE_BUILD_TYPE Release) +ENDIF(NOT CMAKE_BUILD_TYPE) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs nav_msgs) + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) + +## Build +add_executable(zed_tracking_sub src/zed_tracking_sub_tutorial.cpp) +target_link_libraries(zed_tracking_sub ${catkin_LIBRARIES}) diff --git a/tutorials/zed_tracking_sub_tutorial/README.md b/tutorials/zed_tracking_sub_tutorial/README.md new file mode 100644 index 00000000..66d81cc5 --- /dev/null +++ b/tutorials/zed_tracking_sub_tutorial/README.md @@ -0,0 +1,10 @@ +# Positional Tracking tutorial + +In this tutorial you will learn how to write a simple node that subscribes to messages of type +`geometry_msgs/PoseStamped` and `nav_msgs/Odometry` to retrieve the position and the orientation of the ZED camera in the `map` and in the `odometry` frames. + +![](../images/tutorial_tracking.png) + +In this tutorial you will learn how to write a simple node that subscribes to messages of type `geometry_msgs/PoseStamped` and `nav_msgs/Odometry` to retrieve the position and the orientation of the ZED camera in the map and in the odometry frames. + +The complete documentation is available on the [Stereolabs website](https://docs.stereolabs.com/integrations/ros/positional_tracking/) diff --git a/tutorials/zed_tracking_sub_tutorial/package.xml b/tutorials/zed_tracking_sub_tutorial/package.xml new file mode 100644 index 00000000..3220734c --- /dev/null +++ b/tutorials/zed_tracking_sub_tutorial/package.xml @@ -0,0 +1,15 @@ + + zed_tracking_sub_tutorial + 2.6.0 + + This package is a tutorial showing how to subscribe to ZED positional tracking data streams + + STEREOLABS + BSD + + catkin + + roscpp + geometry_msgs + nav_msgs + diff --git a/tutorials/zed_tracking_sub_tutorial/src/zed_tracking_sub_tutorial.cpp b/tutorials/zed_tracking_sub_tutorial/src/zed_tracking_sub_tutorial.cpp new file mode 100644 index 00000000..0d06e646 --- /dev/null +++ b/tutorials/zed_tracking_sub_tutorial/src/zed_tracking_sub_tutorial.cpp @@ -0,0 +1,145 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2018, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + +/** + * This tutorial demonstrates simple receipt of ZED odom and pose messages over the ROS system. + */ + +#include +#include +#include + +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" + +#include + +#define RAD2DEG 57.295779513 + +/** + * Subscriber callbacks + */ + +void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) { + + // Camera position in map frame + double tx = msg->pose.pose.position.x; + double ty = msg->pose.pose.position.y; + double tz = msg->pose.pose.position.z; + + // Orientation quaternion + tf2::Quaternion q( + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w); + + // 3x3 Rotation matrix from quaternion + tf2::Matrix3x3 m(q); + + // Roll Pitch and Yaw from rotation matrix + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + // Output the measure + ROS_INFO("Received odom in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f", + msg->header.frame_id.c_str(), + tx, ty, tz, + roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); +} + +void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) { + + // Camera position in map frame + double tx = msg->pose.position.x; + double ty = msg->pose.position.y; + double tz = msg->pose.position.z; + + // Orientation quaternion + tf2::Quaternion q( + msg->pose.orientation.x, + msg->pose.orientation.y, + msg->pose.orientation.z, + msg->pose.orientation.w); + + // 3x3 Rotation matrix from quaternion + tf2::Matrix3x3 m(q); + + // Roll Pitch and Yaw from rotation matrix + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + // Output the measure + ROS_INFO("Received pose in '%s' frame : X: %.2f Y: %.2f Z: %.2f - R: %.2f P: %.2f Y: %.2f", + msg->header.frame_id.c_str(), + tx, ty, tz, + roll * RAD2DEG, pitch * RAD2DEG, yaw * RAD2DEG); +} + +/** + * Node main function + */ +int main(int argc, char** argv) { + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "zed_tracking_subscriber"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called imageCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + ros::Subscriber subOdom = n.subscribe("/zed/odom", 10, odomCallback); + ros::Subscriber subPose = n.subscribe("/zed/pose", 10, poseCallback); + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + ros::spin(); + + return 0; +} diff --git a/tutorials/zed_video_sub_tutorial/CMakeLists.txt b/tutorials/zed_video_sub_tutorial/CMakeLists.txt new file mode 100644 index 00000000..e977fa1f --- /dev/null +++ b/tutorials/zed_video_sub_tutorial/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zed_video_sub_tutorial) + +# if CMAKE_BUILD_TYPE is not specified, take 'Release' as default +IF(NOT CMAKE_BUILD_TYPE) + SET(CMAKE_BUILD_TYPE Release) +ENDIF(NOT CMAKE_BUILD_TYPE) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) + +## Declare a catkin package +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS}) + +## Build +add_executable(zed_video_sub src/zed_video_sub_tutorial.cpp) +target_link_libraries(zed_video_sub ${catkin_LIBRARIES}) diff --git a/tutorials/zed_video_sub_tutorial/README.md b/tutorials/zed_video_sub_tutorial/README.md new file mode 100644 index 00000000..94debb33 --- /dev/null +++ b/tutorials/zed_video_sub_tutorial/README.md @@ -0,0 +1,7 @@ +# ZED video subscription tutorial + +In this tutorial you will learn how to write a simple node that subscribes to messages of type sensor_msgs/Image to retrieve the Left and Right rectified images published by the ZED node. + +![](../images/tutorial_video.png) + +The complete documentation is available on the [Stereolabs website](https://docs.stereolabs.com/integrations/ros/video/) diff --git a/tutorials/zed_video_sub_tutorial/package.xml b/tutorials/zed_video_sub_tutorial/package.xml new file mode 100644 index 00000000..b52e5fee --- /dev/null +++ b/tutorials/zed_video_sub_tutorial/package.xml @@ -0,0 +1,14 @@ + + zed_video_sub_tutorial + 2.6.0 + + This package is a tutorial showing how to subscribe to ZED Video streams + + STEREOLABS + BSD + + catkin + + roscpp + sensor_msgs + diff --git a/tutorials/zed_video_sub_tutorial/src/zed_video_sub_tutorial.cpp b/tutorials/zed_video_sub_tutorial/src/zed_video_sub_tutorial.cpp new file mode 100644 index 00000000..7c3c9fac --- /dev/null +++ b/tutorials/zed_video_sub_tutorial/src/zed_video_sub_tutorial.cpp @@ -0,0 +1,90 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2018, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + +/** + * This tutorial demonstrates how to receive the Left and Right rectified images + * from the ZED node + */ + +#include +#include + +/** + * Subscriber callbacks. The argument of the callback is a constant pointer to the received message + */ + +void imageRightRectifiedCallback(const sensor_msgs::Image::ConstPtr& msg) { + ROS_INFO("Right Rectified image received from ZED - Size: %dx%d", msg->width, msg->height); +} + +void imageLeftRectifiedCallback(const sensor_msgs::Image::ConstPtr& msg) { + ROS_INFO("Left Rectified image received from ZED - Size: %dx%d", msg->width, msg->height); +} + +/** + * Node main function + */ +int main(int argc, char** argv) { + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "zed_video_subscriber"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called imageCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + ros::Subscriber subRightRectified = n.subscribe("/zed/right/image_rect_color", 10, imageRightRectifiedCallback); + ros::Subscriber subLeftRectified = n.subscribe("/zed/left/image_rect_color", 10, imageLeftRectifiedCallback); + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + ros::spin(); + + return 0; +} diff --git a/zed_display_rviz/launch/display.launch b/zed_display_rviz/launch/display.launch index e27842c8..67f45814 100644 --- a/zed_display_rviz/launch/display.launch +++ b/zed_display_rviz/launch/display.launch @@ -17,13 +17,15 @@ THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. --> + - - - - + + + + + - - + + diff --git a/zed_display_rviz/package.xml b/zed_display_rviz/package.xml index 468d24b1..690430df 100644 --- a/zed_display_rviz/package.xml +++ b/zed_display_rviz/package.xml @@ -1,11 +1,11 @@ zed_display_rviz - 1.0.0 + 2.6.0 "zed_display" is a ROS package to visualize in Rviz the information from the "zed_wrapper" node - STEREOLABS + STEREOLABS BSD catkin diff --git a/zed_display_rviz/rviz/zed.rviz b/zed_display_rviz/rviz/zed.rviz index 931d66c1..1e92abb7 100644 --- a/zed_display_rviz/rviz/zed.rviz +++ b/zed_display_rviz/rviz/zed.rviz @@ -1,17 +1,17 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /RobotModel1/Links1 - - /TF1 - /TF1/Frames1 - - /Odometry1 - - /Odometry1/Shape1 + - /Video1/Camera view1 + - /Video1/Camera view1/Visibility1 - /Pose1 - Splitter Ratio: 0.5 - Tree Height: 452 + Splitter Ratio: 0.409937888 + Tree Height: 459 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -30,7 +30,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: DepthCloud + SyncSource: Camera view Visualization Manager: Class: "" Displays: @@ -91,7 +91,7 @@ Visualization Manager: Visual Enabled: true - Class: rviz/TF Enabled: true - Frame Timeout: 15 + Frame Timeout: 5 Frames: All Enabled: false map: @@ -108,9 +108,9 @@ Visualization Manager: Value: false zed_right_camera_optical_frame: Value: false - Marker Scale: 1 + Marker Scale: 0.5 Name: TF - Show Arrows: true + Show Arrows: false Show Axes: true Show Names: true Tree: @@ -125,150 +125,280 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: true - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/DepthCloud - Color: 255; 255; 255 - Color Image Topic: /zed/rgb/image_raw_color - Color Transformer: RGB8 - Color Transport Hint: raw - Decay Time: 0 - Depth Map Topic: /zed/depth/depth_registered - Depth Map Transport Hint: raw + - Class: rviz/Group + Displays: + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /zed/rgb/image_rect_color + Name: Camera view + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: true + Value: true + Visibility: + Depth: + Confidence image: true + Confidence map: true + Depth map: true + DepthCloud: true + PointCloud2: true + Value: false + Grid: true + Pose: + Odometry: true + Odometry Path: true + Pose: true + Pose Path: true + Value: false + RobotModel: false + TF: true + Value: true + Video: + Left camera: true + Right camera: true + Value: true + Zoom Factor: 1 + - Class: rviz/Image + Enabled: false + Image Topic: /zed/left/image_rect_color + Max Value: 10 + Median window: 5 + Min Value: 0 + Name: Left camera + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /zed/right/image_rect_color + Max Value: 10 + Median window: 5 + Min Value: 0 + Name: Right camera + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: DepthCloud - Occlusion Compensation: - Occlusion Time-Out: 30 - Value: false - Position Transformer: XYZ - Queue Size: 5 - Selectable: true - Size (Pixels): 1 - Style: Points - Topic Filter: true - Use Fixed Frame: true - Use rainbow: true - 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: RGB8 - Decay Time: 0 - Enabled: false - 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: 5 - Selectable: true - Size (Pixels): 1 - Size (m): 0.00999999978 - Style: Points - Topic: /zed/point_cloud/cloud_registered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Camera + Name: Video + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.240854248 + Min Value: -1.55063653 + Value: true + Axis: X + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + 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: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.00999999978 + Style: Points + Topic: /zed/point_cloud/cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/DepthCloud + Color: 255; 255; 255 + Color Image Topic: /zed/rgb/image_rect_color + Color Transformer: RGB8 + Color Transport Hint: raw + Decay Time: 0 + Depth Map Topic: /zed/depth/depth_registered + Depth Map Transport Hint: raw + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: XYZ + Queue Size: 2 + Selectable: true + Size (Pixels): 1 + Style: Points + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /zed/depth/depth_registered + Max Value: 5 + Median window: 5 + Min Value: 0 + Name: Depth map + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /zed/confidence/confidence_image + Max Value: 10 + Median window: 5 + Min Value: 0 + Name: Confidence image + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /zed/confidence/confidence_map + Max Value: 100 + Median window: 5 + Min Value: 0 + Name: Confidence map + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false Enabled: true - Image Rendering: background and overlay - Image Topic: /zed/rgb/image_rect_color - Name: Camera - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Visibility: - Camera: true - DepthCloud: true - Grid: true - Odometry: true - PointCloud2: true - Pose: true - RobotModel: true - TF: true - Value: false - Zoom Factor: 1 - - Angle Tolerance: 0 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 + 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 - Position: - Alpha: 0.300000012 - Color: 204; 51; 204 - Scale: 1 + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.300000012 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 0.100000001 + Axes Radius: 0.00999999978 + Color: 255; 25; 0 + Head Length: 0.100000001 + Head Radius: 0.0500000007 + Shaft Length: 0.300000012 + Shaft Radius: 0.0199999996 + Value: Arrow + Topic: /zed/odom + Unreliable: false + Value: true + - 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: true + Head Length: 0.100000001 + Head Radius: 0.0500000007 + Name: Pose + Shaft Length: 0.300000012 + Shaft Radius: 0.0199999996 + Shape: Arrow + Topic: /zed/pose + Unreliable: false Value: true - Value: true - Enabled: true - Keep: 1 - Name: Odometry - Position Tolerance: 0 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Color: 25; 255; 0 - Head Length: 0.100000001 - Head Radius: 0.100000001 - Shaft Length: 0.300000012 - Shaft Radius: 0.0199999996 - Value: Arrow - Topic: /zed/odom - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Class: rviz/Pose - Color: 255; 25; 0 Enabled: true - Head Length: 0.100000001 - Head Radius: 0.100000001 Name: Pose - Shaft Length: 0.300000012 - Shaft Radius: 0.0199999996 - Shape: Arrow - Topic: /zed/map - Unreliable: false - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: map - Frame Rate: 30 + Frame Rate: 60 Name: root Tools: - Class: rviz/Interact @@ -288,35 +418,45 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 7.54095364 + Distance: 1.35755372 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.946748376 - Y: 0.731980622 - Z: 0.698233485 + X: 0.763631225 + Y: 1.1376406 + Z: 0.689015388 Focal Shape Fixed Size: false Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 0.170398071 - Target Frame: zed_left_camera + Pitch: 0.0447975472 + Target Frame: Value: Orbit (rviz) - Yaw: 2.91542745 + Yaw: 1.85410023 Saved: ~ Window Geometry: - Camera: + Camera view: + collapsed: false + Confidence image: + collapsed: false + Confidence map: + collapsed: false + Depth map: collapsed: false Displays: collapsed: false Height: 1056 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000253000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002400000010f0000000000000000fb0000000a0049006d006100670065000000028b000000c40000000000000000fb0000000a0049006d006100670065010000028a000000c50000000000000000fb0000000a0049006d00610067006500000002b30000009c0000000000000000fb0000000a0049006d006100670065010000028b000000c40000000000000000fb0000000c00430061006d00650072006101000002810000013d0000001600ffffff000000010000010f00000327fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000327000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + Left camera: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f200000394fc0200000013fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000020c000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002400000010f0000000000000000fb0000000a0049006d006100670065000000028b000000c40000000000000000fb0000000a0049006d006100670065010000028a000000c50000000000000000fb0000000a0049006d00610067006500000002b30000009c0000000000000000fb0000000a0049006d006100670065010000028b000000c40000000000000000fb0000001600430061006d00650072006100200076006900650077010000023a000001820000001600fffffffb00000012004400650070007400680020006d00610070020000024f000000be0000016b00000104fb000000200043006f006e0066006900640065006e0063006500200069006d006100670065020000024c000001c50000016e00000104fb0000001c0043006f006e0066006900640065006e006300650020006d00610070020000024c000002cb0000016e00000102fb00000016004c006500660074002000630061006d00650072006100000002510000009b0000001600fffffffb0000001800520069006700680074002000630061006d00650072006100000002c6000000f60000001600ffffff000000010000010f00000394fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000394000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000040fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004320000039400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Right camera: + collapsed: false Selection: collapsed: false Time: @@ -324,7 +464,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true + collapsed: false Width: 1855 X: 65 Y: 24 diff --git a/zed_display_rviz/rviz/zedm.rviz b/zed_display_rviz/rviz/zedm.rviz index 69fa257d..a55ffa87 100644 --- a/zed_display_rviz/rviz/zedm.rviz +++ b/zed_display_rviz/rviz/zedm.rviz @@ -7,10 +7,19 @@ Panels: - /Global Options1 - /RobotModel1/Links1 - /TF1/Frames1 - - /DepthCloud1/Occlusion Compensation1 - - /Odometry1/Covariance1/Position1 + - /Video1/Camera view1 + - /Video1/Camera view1/Visibility1 + - /Depth1/DepthCloud1 + - /Pose1 + - /Pose1/Odometry1/Covariance1 + - /Pose1/Odometry1/Covariance1/Position1 + - /Pose1/Odometry1/Covariance1/Orientation1 + - /Pose1/PoseWithCovariance1 + - /Pose1/PoseWithCovariance1/Covariance1 + - /Pose1/PoseWithCovariance1/Covariance1/Position1 + - /Pose1/PoseWithCovariance1/Covariance1/Orientation1 Splitter Ratio: 0.5 - Tree Height: 393 + Tree Height: 421 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -29,7 +38,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: DepthCloud + SyncSource: Camera view Visualization Manager: Class: "" Displays: @@ -98,7 +107,7 @@ Visualization Manager: Frames: All Enabled: false imu_link: - Value: false + Value: true map: Value: true odom: @@ -132,167 +141,329 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Alpha: 1 - Auto Size: - Auto Size Factor: 1 - Value: true - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/DepthCloud - Color: 255; 255; 255 - Color Image Topic: /zed/rgb/image_rect_color - Color Transformer: RGB8 - Color Transport Hint: raw - Decay Time: 0 - Depth Map Topic: /zed/depth/depth_registered - Depth Map Transport Hint: raw - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: DepthCloud - Occlusion Compensation: - Occlusion Time-Out: 30 - Value: false - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 1 - Style: Points - Topic Filter: true - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Camera + - Class: rviz/Group + Displays: + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /zed/rgb/image_rect_color + Name: Camera view + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: true + Value: true + Visibility: + Depth: + Confidence image: true + Confidence map: true + Depth map: true + DepthCloud: true + PointCloud2: true + Value: false + Grid: true + Pose: + Imu: true + Odometry: true + Odometry Path: true + Pose: true + Pose Path: true + PoseWithCovariance: true + Value: false + RobotModel: false + TF: true + Value: true + Video: + Left camera: true + Right camera: true + Value: true + Zoom Factor: 1 + - Class: rviz/Image + Enabled: false + Image Topic: /zed/left/image_rect_color + Max Value: 10 + Median window: 5 + Min Value: 0 + Name: Left camera + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /zed/right/image_rect_color + Max Value: 10 + Median window: 5 + Min Value: 0 + Name: Right camera + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false Enabled: true - Image Rendering: background and overlay - Image Topic: /zed/rgb/image_rect_color - Name: Camera - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: true - Value: true - Visibility: - DepthCloud: true - Grid: true - Imu: true - Odometry: true - PointCloud2: true - Pose: true - RobotModel: true - TF: true - Value: false - Zoom Factor: 1 - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 0.240854248 - Min Value: -1.55063653 - Value: true - Axis: X - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 + Name: Video + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.240854248 + Min Value: -1.55063653 + Value: true + Axis: X + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + 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: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.00999999978 + Style: Points + Topic: /zed/point_cloud/cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Auto Size: + Auto Size Factor: 1 + Value: true + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/DepthCloud + Color: 255; 255; 255 + Color Image Topic: /zed/rgb/image_rect_color + Color Transformer: RGB8 + Color Transport Hint: raw + Decay Time: 0 + Depth Map Topic: /zed/depth/depth_registered + Depth Map Transport Hint: raw + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: DepthCloud + Occlusion Compensation: + Occlusion Time-Out: 30 + Value: false + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 1 + Style: Points + Topic Filter: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /zed/depth/depth_registered + Max Value: 10 + Median window: 5 + Min Value: 0 + Name: Depth map + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /zed/confidence/confidence_image + Max Value: 10 + Median window: 5 + Min Value: 0 + Name: Confidence image + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /zed/confidence/confidence_map + Max Value: 100 + Median window: 5 + Min Value: 0 + Name: Confidence map + Normalize Range: false + Queue Size: 1 + Transport Hint: raw + Unreliable: true + Value: false Enabled: false - 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: 1 - Selectable: false - Size (Pixels): 1 - Size (m): 0.00999999978 - Style: Points - Topic: /zed/point_cloud/cloud_registered - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Acceleration properties: - Acc. vector alpha: 1 - Acc. vector color: 255; 0; 0 - Acc. vector scale: 1 - Derotate acceleration: false - Enable acceleration: false - Axes properties: - Axes scale: 1 - Enable axes: true - Box properties: - Box alpha: 1 - Box color: 255; 0; 0 - Enable box: false - x_scale: 1 - y_scale: 1 - z_scale: 1 - Class: rviz_imu_plugin/Imu - Enabled: true - Name: Imu - Topic: /zed/imu/data - Unreliable: false - Value: true - fixed_frame_orientation: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Class: rviz/Pose - Color: 0; 255; 0 - Enabled: true - Head Length: 0.100000001 - Head Radius: 0.0500000007 - Name: Pose - Shaft Length: 0.300000012 - Shaft Radius: 0.0199999996 - Shape: Arrow - Topic: /zed/map - Unreliable: false - Value: true - - Angle Tolerance: 0 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 + 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: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: RGB + Frame: Local + Offset: 0.150000006 + Scale: 20 + Value: true + Position: + Alpha: 0.300000012 + Color: 204; 51; 204 + Scale: 10 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 0.100000001 + Axes Radius: 0.00999999978 + Color: 255; 25; 0 + Head Length: 0.100000001 + Head Radius: 0.0500000007 + Shaft Length: 0.300000012 + Shaft Radius: 0.0199999996 + Value: Arrow + Topic: /zed/odom + Unreliable: false Value: true - Position: - Alpha: 0.300000012 - Color: 204; 51; 204 - Scale: 1 + - 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 - Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Class: rviz/Pose + Color: 0; 255; 0 + Enabled: true + Head Length: 0.100000001 + Head Radius: 0.0500000007 + Name: Pose + Shaft Length: 0.300000012 + Shaft Radius: 0.0199999996 + Shape: Arrow + Topic: /zed/pose + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.100000001 + Class: rviz/PoseWithCovariance + Color: 25; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 0.150000006 + Scale: 100 + Value: true + Position: + Alpha: 0.300000012 + Color: 204; 51; 204 + Scale: 100 + Value: true + Value: true + Enabled: false + Head Length: 0.100000001 + Head Radius: 0.0500000007 + Name: PoseWithCovariance + Shaft Length: 0.300000012 + Shaft Radius: 0.0199999996 + Shape: Arrow + Topic: /zed/pose_with_covariance + Unreliable: false + Value: false + - Acceleration properties: + Acc. vector alpha: 1 + Acc. vector color: 255; 0; 0 + Acc. vector scale: 1 + Derotate acceleration: false + Enable acceleration: false + Axes properties: + Axes scale: 1 + Enable axes: true + Box properties: + Box alpha: 1 + Box color: 255; 0; 0 + Enable box: false + x_scale: 1 + y_scale: 1 + z_scale: 1 + Class: rviz_imu_plugin/Imu + Enabled: true + Name: Imu + Topic: /zed/imu/data + Unreliable: false + Value: true + fixed_frame_orientation: true Enabled: true - Keep: 1 - Name: Odometry - Position Tolerance: 0 - Shape: - Alpha: 1 - Axes Length: 0.100000001 - Axes Radius: 0.00999999978 - Color: 255; 25; 0 - Head Length: 0.100000001 - Head Radius: 0.0500000007 - Shaft Length: 0.300000012 - Shaft Radius: 0.0199999996 - Value: Arrow - Topic: /zed/odom - Unreliable: false - Value: true + Name: Pose Enabled: true Global Options: Background Color: 48; 48; 48 @@ -318,35 +489,45 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.1419313 + Distance: 1.97160459 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.546764851 - Y: -0.0699039996 - Z: 0.35577035 + X: -0.156546906 + Y: -0.1158977 + Z: 0.691972911 Focal Shape Fixed Size: false Focal Shape Size: 0.0500000007 Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 0.30479762 + Pitch: 1.04539788 Target Frame: Value: Orbit (rviz) - Yaw: 2.96410108 + Yaw: 3.37540507 Saved: ~ Window Geometry: - Camera: + Camera view: + collapsed: false + Confidence image: + collapsed: false + Confidence map: + collapsed: false + Depth map: collapsed: false Displays: collapsed: false Height: 1056 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000394fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000218000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002400000010f0000000000000000fb0000000a0049006d006100670065000000028b000000c40000000000000000fb0000000a0049006d006100670065010000028a000000c50000000000000000fb0000000a0049006d00610067006500000002b30000009c0000000000000000fb0000000a0049006d006100670065010000028b000000c40000000000000000fb0000000c00430061006d0065007200610100000246000001760000001600ffffff000000010000010f00000394fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000394000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000040fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Left camera: + collapsed: false + QMainWindow State: 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 + Right camera: + collapsed: false Selection: collapsed: false Time: diff --git a/zed_nodelet_example/README.md b/zed_nodelet_example/README.md deleted file mode 100644 index bc1e07aa..00000000 --- a/zed_nodelet_example/README.md +++ /dev/null @@ -1,22 +0,0 @@ -# Stereolabs ZED Camera - ROS Nodelet example - -`zed_nodelet_example` is a ROS package to illustrate how to load the `ZEDWrapperNodelet` with an external nodelet manager and use the intraprocess communication to generate a virtual laser scan thanks to the nodelet `depthimage_to_laserscan` - -### Run the program - -To launch the wrapper nodelet along with the `depthimage_to_laserscan` nodelet, open a terminal and launch: - -`$ roslaunch zed_nodelet_example zed_nodelet_laserscan.launch` - -**Note**: Remember to change the parameter `camera_model` to `0` if you are using a **ZED** or to `1` if you are using a **ZED Mini** - -## Visualization -To visualize the result of the process open Rviz, add a `LaserScan` visualization and set `/zed/scan` as `topic` parameter - -Virtual 2D laser scan rendered in Rviz. You can see the projection of the virtual laser scan on the RGB image on the left -![Virtual laser scan rendered in Rviz](images/laserscan.png) - -Virtual 2D laser scan rendered in Rviz over the 3D depth cloud. You can see the projection of the virtual laser scan on the RGB image on the left -![Virtual laser scan rendered in Rviz on the Depthcloud](images/laserscan-depthcloud.png) - -[More](https://www.stereolabs.com/documentation/guides/using-zed-with-ros/introduction.html) diff --git a/zed_ros/CMakeLists.txt b/zed_ros/CMakeLists.txt new file mode 100644 index 00000000..b983ecce --- /dev/null +++ b/zed_ros/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zed_ros) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/zed_ros/package.xml b/zed_ros/package.xml new file mode 100644 index 00000000..8c1e7a6a --- /dev/null +++ b/zed_ros/package.xml @@ -0,0 +1,22 @@ + + + zed_ros + 2.6.0 + Stereolabs zed-ros-wrapper support meta package + + STEREOLABS + BSD + + http://stereolabs.com/rc_visard + https://github.com/stereolabs/zed-ros-wrapper + https://github.com/stereolabs/zed-ros-wrapper/issues + + catkin + + zed_wrapper + zed_display_rviz + + + + + diff --git a/zed_rtabmap_example/README.md b/zed_rtabmap_example/README.md deleted file mode 100644 index d83e15c1..00000000 --- a/zed_rtabmap_example/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# Stereolabs ZED Camera - RTAB-map example - -This package shows how to use the ZED Wrapper with [RTAB-map](http://introlab.github.io/rtabmap/) - -### Run the program - -To launch the example, open a terminal and launch: - - $ roslaunch zed_rtabmap_example zed_rtabmap.launch - -Example of indoor 3D mapping using RTAB-map and ZED - -![Example of indoor 3D mapping](images/rtab-map.png) - diff --git a/zed_wrapper/cfg/Zed.cfg b/zed_wrapper/cfg/Zed.cfg index 816893f5..44250529 100755 --- a/zed_wrapper/cfg/Zed.cfg +++ b/zed_wrapper/cfg/Zed.cfg @@ -5,10 +5,11 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80, 1, 100) -gen.add("exposure", int_t, 1, "Exposure value when manual controlled", 100, 0, 100); -gen.add("gain", int_t, 2, "Gain value when manual controlled", 50, 0, 100); -gen.add("auto_exposure", bool_t, 3, "Enable/Disable auto control of exposure and gain", True); -gen.add("mat_resize_factor", double_t, 4, "Image/Measures resize factor", 1.0, 0.1, 1.0); +gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80, 1, 100) +gen.add("exposure", int_t, 1, "Exposure value when manual controlled", 100, 0, 100); +gen.add("gain", int_t, 2, "Gain value when manual controlled", 50, 0, 100); +gen.add("auto_exposure", bool_t, 3, "Enable/Disable auto control of exposure and gain", True); +gen.add("mat_resize_factor", double_t, 4, "Image/Measures resize factor", 1.0, 0.1, 1.0); +gen.add("max_depth", double_t, 5, "Maximum Depth Range", 3.5, 0.5, 20.0); exit(gen.generate(PACKAGE, "zed_wrapper", "Zed")) diff --git a/zed_wrapper/launch/README.md b/zed_wrapper/launch/README.md index ccfd6be1..de18165d 100644 --- a/zed_wrapper/launch/README.md +++ b/zed_wrapper/launch/README.md @@ -1,3 +1,5 @@ +![](../images/Picto+STEREOLABS_Black.png) + # Launch Files Launch files provide a convenient way to start up multiple nodes and a master, as well as setting parameters. diff --git a/zed_wrapper/launch/zed_camera.launch b/zed_wrapper/launch/zed_camera.launch index af2968e7..3b4de0e9 100644 --- a/zed_wrapper/launch/zed_camera.launch +++ b/zed_wrapper/launch/zed_camera.launch @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -62,6 +63,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + @@ -93,12 +96,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + - + + $(arg initial_pose) @@ -127,16 +132,21 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + - + + + + + + diff --git a/zed_wrapper/launch/zed_camera_nodelet.launch b/zed_wrapper/launch/zed_camera_nodelet.launch index 8e0c5866..793019e2 100644 --- a/zed_wrapper/launch/zed_camera_nodelet.launch +++ b/zed_wrapper/launch/zed_camera_nodelet.launch @@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -64,6 +65,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + @@ -94,12 +96,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + $(arg initial_pose) @@ -128,16 +132,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - + - + + + + + diff --git a/zed_wrapper/package.xml b/zed_wrapper/package.xml index 19e34df7..f92ff988 100644 --- a/zed_wrapper/package.xml +++ b/zed_wrapper/package.xml @@ -1,11 +1,11 @@ zed_wrapper - 1.0.0 + 2.6.0 zed_wrapper is a ROS wrapper for the ZED library for interfacing with the ZED camera. -STEREOLABS + STEREOLABS BSD catkin @@ -23,6 +23,7 @@ urdf message_generation + robot_state_publisher message_runtime diff --git a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp index 7c98ae1d..d047f85c 100644 --- a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp @@ -25,6 +25,7 @@ ** This sample is a wrapper for the ZED library in order to use the ZED Camera with ROS. ** ** A set of parameters can be specified in the launch file. ** ****************************************************************************************************/ +#include "sl_tools.h" #include @@ -35,6 +36,7 @@ #include #include #include +#include #include #include @@ -61,14 +63,6 @@ namespace zed_wrapper { */ virtual ~ZEDWrapperNodelet(); - /* \brief Image to ros message conversion - * \param img : the image to publish - * \param encodingType : the sensor_msgs::image_encodings encoding type - * \param frameId : the id of the reference frame of the image - * \param t : the ros::Time to stamp the image - */ - static sensor_msgs::ImagePtr imageToROSmsg(sl::Mat img, std::string frameId, ros::Time t); - private: /* \brief Initialization function called by the Nodelet base class */ @@ -76,48 +70,55 @@ namespace zed_wrapper { /* \brief ZED camera polling thread function */ - void device_poll(); + void device_poll_thread_func(); /* \brief Pointcloud publishing function */ - void pointcloud_thread(); + void pointcloud_thread_func(); protected: /* \brief Publish the pose of the camera in "Map" frame with a ros Publisher - * \param poseBaseTransform : Transformation representing the camera pose from odom frame to map frame * \param t : the ros::Time to stamp the image */ - void publishPose(tf2::Transform poseBaseTransform, ros::Time t); + void publishPose(ros::Time t); /* \brief Publish the pose of the camera in "Odom" frame with a ros Publisher - * \param odom_base_transform : Transformation representing the camera pose from base frame to odom frame + * \param base2odomTransf : Transformation representing the camera pose + * from base frame to odom frame + * \param slPose : latest odom pose from ZED SDK * \param t : the ros::Time to stamp the image */ - void publishOdom(tf2::Transform baseToOdomTransform, ros::Time t); + void publishOdom(tf2::Transform base2odomTransf, sl::Pose& slPose, ros::Time t); /* \brief Publish the pose of the camera in "Map" frame as a transformation - * \param base_transform : Transformation representing the camera pose from odom frame to map frame + * \param baseTransform : Transformation representing the camera pose from + * odom frame to map frame * \param t : the ros::Time to stamp the image */ void publishPoseFrame(tf2::Transform baseTransform, ros::Time t); - /* \brief Publish the odometry of the camera in "Odom" frame as a transformation - * \param base_transform : Transformation representing the camera pose from base frame to odom frame + /* \brief Publish the odometry of the camera in "Odom" frame as a + * transformation + * \param odomTransf : Transformation representing the camera pose from + * base frame to odom frame * \param t : the ros::Time to stamp the image */ - void publishOdomFrame(tf2::Transform baseToOdomTransform, ros::Time t); + void publishOdomFrame(tf2::Transform odomTransf, ros::Time t); /* \brief Publish the pose of the imu in "Odom" frame as a transformation - * \param base_transform : Transformation representing the imu pose from base frame to odom frame + * \param imuTransform : Transformation representing the imu pose from base + * frame to odom framevoid * \param t : the ros::Time to stamp the image */ - void publishImuFrame(tf2::Transform baseTransform); + void publishImuFrame(tf2::Transform imuTransform, ros::Time t); /* \brief Publish a sl::Mat image with a ros Publisher * \param img : the image to publish - * \param pub_img : the publisher object to use (different image publishers exist) - * \param img_frame_id : the id of the reference frame of the image (different image frames exist) + * \param pub_img : the publisher object to use (different image publishers + * exist) + * \param img_frame_id : the id of the reference frame of the image (different + * image frames exist) * \param t : the ros::Time to stamp the image */ void publishImage(sl::Mat img, image_transport::Publisher& pubImg, string imgFrameId, ros::Time t); @@ -135,8 +136,6 @@ namespace zed_wrapper { void publishConf(sl::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 */ void publishPointCloud(); @@ -145,7 +144,8 @@ namespace zed_wrapper { * \param pub_cam_info : the publisher object to use * \param t : the ros::Time to stamp the message */ - void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); + void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, + ros::Publisher pubCamInfo, ros::Time t); /* \brief Publish a sl::Mat disparity image with a ros Publisher * \param disparity : the disparity image to publish @@ -153,33 +153,50 @@ namespace zed_wrapper { */ void publishDisparity(sl::Mat disparity, ros::Time t); - /* \brief Get the information of the ZED cameras and store them in an information message + /* \brief Get the information of the ZED cameras and store them in an + * information message * \param zed : the sl::zed::Camera* pointer to an instance - * \param left_cam_info_msg : the information message to fill with the left camera informations - * \param right_cam_info_msg : the information message to fill with the right camera informations + * \param left_cam_info_msg : the information message to fill with the left + * camera informations + * \param right_cam_info_msg : the information message to fill with the right + * camera informations * \param left_frame_id : the id of the reference frame of the left camera * \param right_frame_id : the id of the reference frame of the right camera */ - void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, sensor_msgs::CameraInfoPtr rightCamInfoMsg, - string leftFrameId, string rightFrameId, bool rawParam = false); + void fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, + sensor_msgs::CameraInfoPtr rightCamInfoMsg, + string leftFrameId, string rightFrameId, + bool rawParam = false); + + /* \bried Check if FPS and Resolution chosen by user are correct. + * Modifies FPS to match correct value. + */ + void checkResolFps(); /* \brief Callback to handle dynamic reconfigure events in ROS */ void dynamicReconfCallback(zed_wrapper::ZedConfig& config, uint32_t level); + /* \brief Callback to publish Path data with a ROS publisher. + * \param e : the ros::TimerEvent binded to the callback + */ + void pathPubCallback(const ros::TimerEvent& e); + /* \brief Callback to publish IMU raw data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ void imuPubCallback(const ros::TimerEvent& e); /* \brief Service callback to reset_tracking service - * Tracking pose is reinitialized to the value available in the ROS Param server + * Tracking pose is reinitialized to the value available in the ROS Param + * server */ bool on_reset_tracking(zed_wrapper::reset_tracking::Request& req, zed_wrapper::reset_tracking::Response& res); /* \brief Service callback to reset_odometry service - * Odometry is reset to clear drift and odometry frame gets the latest pose + * Odometry is reset to clear drift and odometry frame gets the latest + * pose * from ZED tracking. */ bool on_reset_odometry(zed_wrapper::reset_odometry::Request& req, @@ -195,177 +212,206 @@ namespace zed_wrapper { */ void set_pose(float xt, float yt, float zt, float rr, float pr, float yr); + /* \brief Utility to initialize the most used transforms + */ + void initTransforms(); + /* \bried Start tracking loading the parameters from param server */ void start_tracking(); - /* \bried Check if FPS and Resolution chosen by user are correct. - * Modifies FPS to match correct value. - */ - void checkResolFps(); + private: // SDK version - int verMajor; - int verMinor; - int verSubMinor; + int mVerMajor; + int mVerMinor; + int mVerSubMinor; // ROS - ros::NodeHandle nh; - ros::NodeHandle nhNs; - std::thread devicePollThread; + ros::NodeHandle mNh; + ros::NodeHandle mNhNs; + std::thread mDevicePollThread; std::thread mPcThread; // Point Cloud thread bool mStopNode; // Publishers - image_transport::Publisher pubRgb; - image_transport::Publisher pubRawRgb; - image_transport::Publisher pubLeft; - image_transport::Publisher pubRawLeft; - image_transport::Publisher pubRight; - image_transport::Publisher pubRawRight; - image_transport::Publisher pubDepth; - image_transport::Publisher pubConfImg; - ros::Publisher pubConfMap; - ros::Publisher pubDisparity; - ros::Publisher pubCloud; - ros::Publisher pubRgbCamInfo; - ros::Publisher pubLeftCamInfo; - ros::Publisher pubRightCamInfo; - ros::Publisher pubRgbCamInfoRaw; - ros::Publisher pubLeftCamInfoRaw; - ros::Publisher pubRightCamInfoRaw; - ros::Publisher pubDepthCamInfo; - ros::Publisher pubPose; - ros::Publisher pubOdom; - ros::Publisher pubImu; - ros::Publisher pubImuRaw; - ros::Timer pubImuTimer; - - // Service - bool trackingActivated; - ros::ServiceServer srvSetInitPose; - ros::ServiceServer srvResetOdometry; - ros::ServiceServer srvResetTracking; + image_transport::Publisher mPubRgb; // + image_transport::Publisher mPubRawRgb; // + image_transport::Publisher mPubLeft; // + image_transport::Publisher mPubRawLeft; // + image_transport::Publisher mPubRight; // + image_transport::Publisher mPubRawRight; // + image_transport::Publisher mPubDepth; // + image_transport::Publisher mPubConfImg; // + + ros::Publisher mPubConfMap; // + ros::Publisher mPubDisparity; // + ros::Publisher mPubCloud; + ros::Publisher mPubRgbCamInfo; // + ros::Publisher mPubLeftCamInfo; // + ros::Publisher mPubRightCamInfo; // + ros::Publisher mPubRgbCamInfoRaw; // + ros::Publisher mPubLeftCamInfoRaw; // + ros::Publisher mPubRightCamInfoRaw; // + ros::Publisher mPubDepthCamInfo; // + ros::Publisher mPubPose; + ros::Publisher mPubPoseCov; + ros::Publisher mPubOdom; + ros::Publisher mPubOdomPath; + ros::Publisher mPubMapPath; + ros::Publisher mPubImu; + ros::Publisher mPubImuRaw; + //ros::Publisher mPubClock; + + // Timers + ros::Timer mImuTimer; + ros::Timer mPathTimer; + + // Services + ros::ServiceServer mSrvSetInitPose; + ros::ServiceServer mSrvResetOdometry; + ros::ServiceServer mSrvResetTracking; + // Camera info - sensor_msgs::CameraInfoPtr rgbCamInfoMsg; - sensor_msgs::CameraInfoPtr leftCamInfoMsg; - sensor_msgs::CameraInfoPtr rightCamInfoMsg; - sensor_msgs::CameraInfoPtr rgbCamInfoRawMsg; - sensor_msgs::CameraInfoPtr leftCamInfoRawMsg; - sensor_msgs::CameraInfoPtr rightCamInfoRawMsg; - sensor_msgs::CameraInfoPtr depthCamInfoMsg; - - // tf - tf2_ros::TransformBroadcaster transformPoseBroadcaster; - tf2_ros::TransformBroadcaster transformOdomBroadcaster; - tf2_ros::TransformBroadcaster transformImuBroadcaster; - - std::string rgbFrameId; - std::string rgbOptFrameId; - - std::string depthFrameId; - std::string depthOptFrameId; - - std::string disparityFrameId; - std::string disparityOptFrameId; - - std::string confidenceFrameId; - std::string confidenceOptFrameId; - - std::string cloudFrameId; - - std::string mapFrameId; - std::string odometryFrameId; - std::string baseFrameId; - std::string rightCamFrameId; - std::string rightCamOptFrameId; - std::string leftCamFrameId; - std::string leftCamOptFrameId; - std::string imuFrameId; + sensor_msgs::CameraInfoPtr mRgbCamInfoMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoMsg; + sensor_msgs::CameraInfoPtr mRgbCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mLeftCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mRightCamInfoRawMsg; + sensor_msgs::CameraInfoPtr mDepthCamInfoMsg; + + // ROS TF + tf2_ros::TransformBroadcaster mTransformPoseBroadcaster; + tf2_ros::TransformBroadcaster mTransformOdomBroadcaster; + tf2_ros::TransformBroadcaster mTransformImuBroadcaster; + + std::string mRgbFrameId; + std::string mRgbOptFrameId; + + std::string mDepthFrameId; + std::string mDepthOptFrameId; + + std::string mDisparityFrameId; + std::string mDisparityOptFrameId; + + std::string mConfidenceFrameId; + std::string mConfidenceOptFrameId; + + std::string mCloudFrameId; + std::string mPointCloudFrameId; + + std::string mMapFrameId; + std::string mOdometryFrameId; + std::string mBaseFrameId; + std::string mCameraFrameId; + + std::string mRightCamFrameId; + std::string mRightCamOptFrameId; + std::string mLeftCamFrameId; + std::string mLeftCamOptFrameId; + std::string mImuFrameId; // initialization Transform listener - std::shared_ptr tfBuffer; - std::shared_ptr tfListener; - bool publishTf; - bool cameraFlip; + boost::shared_ptr mTfBuffer; + boost::shared_ptr mTfListener; + + bool mPublishTf; + bool mPublishMapTf; + bool mCameraFlip; // Launch file parameters - int resolution; - int frameRate; - int quality; - int sensingMode; - int gpuId; - int zedId; - int depthStabilization; - std::string odometryDb; - std::string svoFilepath; - double imuPubRate; - bool verbose; - - // IMU time - ros::Time imuTime; - - bool poseSmoothing; - bool spatialMemory; - bool initOdomWithPose; + int mCamResol; + int mCamFrameRate; + int mCamQuality; + int mCamSensingMode; + int mGpuId; + int mZedId; + int mDepthStabilization; + std::string mOdometryDb; + std::string mSvoFilepath; + double mImuPubRate; + double mPathPubRate; + int mPathMaxCount; + bool mVerbose; + bool mSvoMode = false; + + bool mTrackingActivated; + bool mTrackingReady; + bool mFloorAlignment = false; + + // Last frame time + ros::Time mPrevFrameTimestamp; + ros::Time mFrameTimestamp; //Tracking variables - sl::Transform initialPoseSl; - std::vector initialTrackPose; - - tf2::Transform odomToMapTransform; - tf2::Transform baseToOdomTransform; - - // zed object - sl::InitParameters param; - sl::Camera zed; - unsigned int serial_number; - int userCamModel; // Camera model set by ROS Param - sl::MODEL realCamModel; // Camera model requested to SDK + sl::Pose mLastZedPose; // Sensor to Map transform + sl::Transform mInitialPoseSl; + std::vector mInitialTrackPose; + std::vector mOdomPath; + std::vector mMapPath; + + // TF Transforms + tf2::Transform mOdom2MapTransf; + tf2::Transform mBase2OdomTransf; + tf2::Transform mSensor2BaseTransf; + + // Zed object + sl::InitParameters mZedParams; + sl::Camera mZed; + unsigned int mZedSerialNumber; + int mZedUserCamModel; // Camera model set by ROS Param + sl::MODEL mZedRealCamModel; // Camera model requested to SDK + + // Dynamic Parameters + int mCamConfidence; + int mCamExposure; + int mCamGain; + double mCamMatResizeFactor; + double mCamMaxDepth; + bool mCamAutoExposure; // flags - double matResizeFactor; - int confidence; - int exposure; - int gain; - bool autoExposure; - bool triggerAutoExposure; - bool computeDepth; - bool grabbing = false; - int openniDepthMode = 0; // 16 bit UC data in mm else 32F in m, for more info http://www.ros.org/reps/rep-0118.html - - // Frame and Mat - int camWidth; - int camHeight; - int matWidth; - int matHeight; - - // Mutex - std::mutex dataMutex; + bool mTriggerAutoExposure; + bool mComputeDepth; + bool mOpenniDepthMode; // 16 bit UC data in mm else 32F in m, for more info -> http://www.ros.org/reps/rep-0118.html + bool mPoseSmoothing; + bool mSpatialMemory; + bool mInitOdomWithPose; + bool mResetOdom = false; + bool mPublishPoseCovariance = true; + + // Mat + int mCamWidth; + int mCamHeight; + int mMatWidth; + int mMatHeight; + + // Thread Sync + std::mutex mCloseZedMutex; + std::mutex mCamDataMutex; std::mutex mPcMutex; std::condition_variable mPcDataReadyCondVar; bool mPcDataReady; // Point cloud variables - sl::Mat cloud; + sl::Mat mCloud; sensor_msgs::PointCloud2 mPointcloudMsg; - string pointCloudFrameId = ""; - ros::Time pointCloudTime; + ros::Time mPointCloudTime; // Dynamic reconfigure - boost::shared_ptr> server; + boost::shared_ptr> mDynRecServer; // Coordinate Changing indices and signs - unsigned int xIdx, yIdx, zIdx; - int xSign, ySign, zSign; - + int mIdxX, mIdxY, mIdxZ; + int mSignX, mSignY, mSignZ; }; // class ZEDROSWrapperNodelet } // namespace #include PLUGINLIB_EXPORT_CLASS(zed_wrapper::ZEDWrapperNodelet, nodelet::Nodelet) -#endif // ZED_WRAPPER_NODELET_H +#endif // ZED_WRAPPER_NODELET_H \ No newline at end of file diff --git a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp index bde6163d..de5cebd9 100644 --- a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -17,30 +17,23 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // /////////////////////////////////////////////////////////////////////////// + #include "zed_wrapper_nodelet.hpp" -#include "sl_tools.h" #ifndef NDEBUG #include #endif +#include #include +#include #include #include #include #include #include - #include -// >>>>> Backward compatibility -#define COORDINATE_SYSTEM_IMAGE static_cast(0) -#define COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP \ - static_cast(3) -#define COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD \ - static_cast(5) -// <<<<< Backward compatibility - using namespace std; namespace zed_wrapper { @@ -48,8 +41,8 @@ namespace zed_wrapper { ZEDWrapperNodelet::ZEDWrapperNodelet() : Nodelet() {} ZEDWrapperNodelet::~ZEDWrapperNodelet() { - if (devicePollThread.joinable()) { - devicePollThread.join(); + if (mDevicePollThread.joinable()) { + mDevicePollThread.join(); } if (mPcThread.joinable()) { @@ -63,608 +56,617 @@ namespace zed_wrapper { mPcDataReady = false; #ifndef NDEBUG + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged(); } #endif - // Launch file parameters - resolution = sl::RESOLUTION_HD720; - quality = sl::DEPTH_MODE_PERFORMANCE; - sensingMode = sl::SENSING_MODE_STANDARD; - frameRate = 30; - gpuId = -1; - zedId = 0; - serial_number = 0; - odometryDb = ""; - imuPubRate = 100.0; - initialTrackPose.resize(6); - for (int i = 0; i < 6; i++) { - initialTrackPose[i] = 0.0f; + mCamResol = sl::RESOLUTION_HD720; + mCamQuality = sl::DEPTH_MODE_PERFORMANCE; + mCamSensingMode = sl::SENSING_MODE_STANDARD; + mCamFrameRate = 30; + mGpuId = -1; + mZedId = 0; + mZedSerialNumber = 0; + mOdometryDb = ""; + mImuPubRate = 100.0; + mPathPubRate = 1.0; + mInitialTrackPose.resize(6); + mOpenniDepthMode = false; + mTrackingReady = false; + + for (size_t i = 0; i < 6; i++) { + mInitialTrackPose[i] = 0.0f; } - matResizeFactor = 1.0; - - verbose = true; - - nh = getMTNodeHandle(); - nhNs = getMTPrivateNodeHandle(); + mCamMatResizeFactor = 1.0; + mVerbose = true; + mNh = getMTNodeHandle(); + mNhNs = getMTPrivateNodeHandle(); // Set default coordinate frames - nhNs.param("pose_frame", mapFrameId, "pose_frame"); - nhNs.param("odometry_frame", odometryFrameId, "odometry_frame"); - nhNs.param("base_frame", baseFrameId, "base_frame"); - nhNs.param("imu_frame", imuFrameId, "imu_link"); - - nhNs.param("left_camera_frame", leftCamFrameId, - "left_camera_frame"); - nhNs.param("left_camera_optical_frame", leftCamOptFrameId, - "left_camera_optical_frame"); - - nhNs.param("right_camera_frame", rightCamFrameId, - "right_camera_frame"); - nhNs.param("right_camera_optical_frame", rightCamOptFrameId, - "right_camera_optical_frame"); - - depthFrameId = leftCamFrameId; - depthOptFrameId = leftCamOptFrameId; + mNhNs.param("pose_frame", mMapFrameId, "map"); + mNhNs.param("odometry_frame", mOdometryFrameId, "odom"); + mNhNs.param("base_frame", mBaseFrameId, "base_link"); + mNhNs.param("camera_frame", mCameraFrameId, "zed_camera_center"); + mNhNs.param("imu_frame", mImuFrameId, "imu_link"); + mNhNs.param("left_camera_frame", mLeftCamFrameId, + "left_camera_frame"); + mNhNs.param("left_camera_optical_frame", mLeftCamOptFrameId, + "left_camera_optical_frame"); + mNhNs.param("right_camera_frame", mRightCamFrameId, + "right_camera_frame"); + mNhNs.param("right_camera_optical_frame", mRightCamOptFrameId, + "right_camera_optical_frame"); + mDepthFrameId = mLeftCamFrameId; + mDepthOptFrameId = mLeftCamOptFrameId; // Note: Depth image frame id must match color image frame id - cloudFrameId = depthOptFrameId; - rgbFrameId = depthFrameId; - rgbOptFrameId = cloudFrameId; - - disparityFrameId = depthFrameId; - disparityOptFrameId = depthOptFrameId; - - confidenceFrameId = depthFrameId; - confidenceOptFrameId = depthOptFrameId; + mCloudFrameId = mDepthOptFrameId; + mRgbFrameId = mDepthFrameId; + mRgbOptFrameId = mCloudFrameId; + mDisparityFrameId = mDepthFrameId; + mDisparityOptFrameId = mDepthOptFrameId; + mConfidenceFrameId = mDepthFrameId; + mConfidenceOptFrameId = mDepthOptFrameId; // Get parameters from launch file - nhNs.getParam("resolution", resolution); - nhNs.getParam("frame_rate", frameRate); - + mNhNs.getParam("resolution", mCamResol); + mNhNs.getParam("frame_rate", mCamFrameRate); checkResolFps(); + mNhNs.getParam("verbose", mVerbose); + mNhNs.getParam("quality", mCamQuality); + mNhNs.getParam("sensing_mode", mCamSensingMode); + mNhNs.getParam("openni_depth_mode", mOpenniDepthMode); + mNhNs.getParam("gpu_id", mGpuId); + mNhNs.getParam("zed_id", mZedId); + mNhNs.getParam("depth_stabilization", mDepthStabilization); - nhNs.getParam("verbose", verbose); - nhNs.getParam("quality", quality); - nhNs.getParam("sensing_mode", sensingMode); - nhNs.getParam("openni_depth_mode", openniDepthMode); - nhNs.getParam("gpu_id", gpuId); - nhNs.getParam("zed_id", zedId); - nhNs.getParam("depth_stabilization", depthStabilization); int tmp_sn = 0; - nhNs.getParam("serial_number", tmp_sn); + mNhNs.getParam("serial_number", tmp_sn); + if (tmp_sn > 0) { - serial_number = tmp_sn; + mZedSerialNumber = static_cast(tmp_sn); } - nhNs.getParam("camera_model", userCamModel); - // Publish odometry tf - nhNs.param("publish_tf", publishTf, true); + mNhNs.getParam("camera_model", mZedUserCamModel); - nhNs.param("camera_flip", cameraFlip, false); + mNhNs.getParam("publish_pose_covariance", mPublishPoseCovariance); - if (serial_number > 0) { - NODELET_INFO_STREAM("SN : " << serial_number); + // Publish odometry tf + mNhNs.param("publish_tf", mPublishTf, true); + mNhNs.param("publish_map_tf", mPublishMapTf, true); + mNhNs.param("camera_flip", mCameraFlip, false); + + if (mZedSerialNumber > 0) { + NODELET_INFO_STREAM("SN : " << mZedSerialNumber); } // Print order frames - NODELET_INFO_STREAM("pose_frame \t\t -> " << mapFrameId); - NODELET_INFO_STREAM("odometry_frame \t\t -> " << odometryFrameId); - NODELET_INFO_STREAM("base_frame \t\t -> " << baseFrameId); - NODELET_INFO_STREAM("imu_link \t\t -> " << imuFrameId); - NODELET_INFO_STREAM("left_camera_frame \t -> " << leftCamFrameId); - NODELET_INFO_STREAM("left_camera_optical_frame -> " << leftCamOptFrameId); - NODELET_INFO_STREAM("right_camera_frame \t -> " << rightCamFrameId); - NODELET_INFO_STREAM("right_camera_optical_frame -> " << rightCamOptFrameId); - NODELET_INFO_STREAM("depth_frame \t\t -> " << depthFrameId); - NODELET_INFO_STREAM("depth_optical_frame \t -> " << depthOptFrameId); - NODELET_INFO_STREAM("disparity_frame \t -> " << disparityFrameId); - NODELET_INFO_STREAM("disparity_optical_frame -> " << disparityOptFrameId); - + NODELET_INFO_STREAM("pose_frame \t\t -> " << mMapFrameId); + NODELET_INFO_STREAM("odometry_frame \t\t -> " << mOdometryFrameId); + NODELET_INFO_STREAM("base_frame \t\t -> " << mBaseFrameId); + NODELET_INFO_STREAM("camera_frame \t\t -> " << mCameraFrameId); + NODELET_INFO_STREAM("imu_link \t\t -> " << mImuFrameId); + NODELET_INFO_STREAM("left_camera_frame \t -> " << mLeftCamFrameId); + NODELET_INFO_STREAM("left_camera_optical_frame -> " << mLeftCamOptFrameId); + NODELET_INFO_STREAM("right_camera_frame \t -> " << mRightCamFrameId); + NODELET_INFO_STREAM("right_camera_optical_frame -> " << mRightCamOptFrameId); + NODELET_INFO_STREAM("depth_frame \t\t -> " << mDepthFrameId); + NODELET_INFO_STREAM("depth_optical_frame \t -> " << mDepthOptFrameId); + NODELET_INFO_STREAM("disparity_frame \t -> " << mDisparityFrameId); + NODELET_INFO_STREAM("disparity_optical_frame -> " << mDisparityOptFrameId); + + NODELET_INFO_STREAM("Camera Flip [" << (mCameraFlip ? "TRUE" : "FALSE") << "]"); + + // Status of odometry TF + NODELET_INFO_STREAM("Broadcasting " << mOdometryFrameId << " [" << (mPublishTf ? "TRUE" : "FALSE") << "]"); // Status of map TF - NODELET_INFO_STREAM("Publish " << mapFrameId << " [" - << (publishTf ? "TRUE" : "FALSE") << "]"); - NODELET_INFO_STREAM("Camera Flip [" << (cameraFlip ? "TRUE" : "FALSE") << "]"); + NODELET_INFO_STREAM("Broadcasting " << mMapFrameId << " [" << ((mPublishTf && + mPublishMapTf) ? "TRUE" : "FALSE") << "]"); std::string img_topic = "image_rect_color"; std::string img_raw_topic = "image_raw_color"; - // Set the default topic names string left_topic = "left/" + img_topic; string left_raw_topic = "left/" + img_raw_topic; string left_cam_info_topic = "left/camera_info"; string left_cam_info_raw_topic = "left/camera_info_raw"; - string right_topic = "right/" + img_topic; string right_raw_topic = "right/" + img_raw_topic; string right_cam_info_topic = "right/camera_info"; string right_cam_info_raw_topic = "right/camera_info_raw"; - string rgb_topic = "rgb/" + img_topic; string rgb_raw_topic = "rgb/" + img_raw_topic; string rgb_cam_info_topic = "rgb/camera_info"; string rgb_cam_info_raw_topic = "rgb/camera_info_raw"; - + string depth_topic = "depth/"; - if (openniDepthMode) { + if (mOpenniDepthMode) { NODELET_INFO_STREAM("Openni depth mode activated"); depth_topic += "depth_raw_registered"; } else { depth_topic += "depth_registered"; } - string depth_cam_info_topic = "depth/camera_info"; - string disparity_topic = "disparity/disparity_image"; - string point_cloud_topic = "point_cloud/cloud_registered"; - string conf_img_topic = "confidence/confidence_image"; string conf_map_topic = "confidence/confidence_map"; - - string pose_topic = "map"; + string pose_topic = "pose"; + string pose_cov_topic = "pose_with_covariance"; string odometry_topic = "odom"; + string odom_path_topic = "path_odom"; + string map_path_topic = "path_map"; string imu_topic = "imu/data"; string imu_topic_raw = "imu/data_raw"; - - nhNs.getParam("rgb_topic", rgb_topic); - nhNs.getParam("rgb_raw_topic", rgb_raw_topic); - nhNs.getParam("rgb_cam_info_topic", rgb_cam_info_topic); - nhNs.getParam("rgb_cam_info_raw_topic", rgb_cam_info_raw_topic); - - nhNs.getParam("left_topic", left_topic); - nhNs.getParam("left_raw_topic", left_raw_topic); - nhNs.getParam("left_cam_info_topic", left_cam_info_topic); - nhNs.getParam("left_cam_info_raw_topic", left_cam_info_raw_topic); - - nhNs.getParam("right_topic", right_topic); - nhNs.getParam("right_raw_topic", right_raw_topic); - nhNs.getParam("right_cam_info_topic", right_cam_info_topic); - nhNs.getParam("right_cam_info_raw_topic", right_cam_info_raw_topic); - - nhNs.getParam("depth_topic", depth_topic); - nhNs.getParam("depth_cam_info_topic", depth_cam_info_topic); - - nhNs.getParam("disparity_topic", disparity_topic); - - nhNs.getParam("confidence_img_topic", conf_img_topic); - nhNs.getParam("confidence_map_topic", conf_map_topic); - - nhNs.getParam("point_cloud_topic", point_cloud_topic); - - nhNs.getParam("pose_topic", pose_topic); - nhNs.getParam("odometry_topic", odometry_topic); - - nhNs.getParam("imu_topic", imu_topic); - nhNs.getParam("imu_topic_raw", imu_topic_raw); - nhNs.getParam("imu_pub_rate", imuPubRate); + mNhNs.getParam("rgb_topic", rgb_topic); + mNhNs.getParam("rgb_raw_topic", rgb_raw_topic); + mNhNs.getParam("rgb_cam_info_topic", rgb_cam_info_topic); + mNhNs.getParam("rgb_cam_info_raw_topic", rgb_cam_info_raw_topic); + mNhNs.getParam("left_topic", left_topic); + mNhNs.getParam("left_raw_topic", left_raw_topic); + mNhNs.getParam("left_cam_info_topic", left_cam_info_topic); + mNhNs.getParam("left_cam_info_raw_topic", left_cam_info_raw_topic); + mNhNs.getParam("right_topic", right_topic); + mNhNs.getParam("right_raw_topic", right_raw_topic); + mNhNs.getParam("right_cam_info_topic", right_cam_info_topic); + mNhNs.getParam("right_cam_info_raw_topic", right_cam_info_raw_topic); + mNhNs.getParam("depth_topic", depth_topic); + mNhNs.getParam("depth_cam_info_topic", depth_cam_info_topic); + mNhNs.getParam("disparity_topic", disparity_topic); + mNhNs.getParam("confidence_img_topic", conf_img_topic); + mNhNs.getParam("confidence_map_topic", conf_map_topic); + mNhNs.getParam("point_cloud_topic", point_cloud_topic); + mNhNs.getParam("pose_topic", pose_topic); + pose_cov_topic = pose_topic + "_with_covariance"; + mNhNs.getParam("odometry_topic", odometry_topic); + mNhNs.getParam("imu_topic", imu_topic); + mNhNs.getParam("imu_topic_raw", imu_topic_raw); + mNhNs.getParam("imu_pub_rate", mImuPubRate); + mNhNs.getParam("path_pub_rate", mPathPubRate); + mNhNs.getParam("path_max_count", mPathMaxCount); + if (mPathMaxCount < 2 && mPathMaxCount != -1) { + mPathMaxCount = 2; + } // Create camera info - sensor_msgs::CameraInfoPtr rgb_cam_info_ms_g(new sensor_msgs::CameraInfo()); + sensor_msgs::CameraInfoPtr rgb_cam_info_msg_(new sensor_msgs::CameraInfo()); sensor_msgs::CameraInfoPtr left_cam_info_msg_(new sensor_msgs::CameraInfo()); sensor_msgs::CameraInfoPtr right_cam_info_msg_(new sensor_msgs::CameraInfo()); - sensor_msgs::CameraInfoPtr rgb_cam_info_raw_msg_( - new sensor_msgs::CameraInfo()); - sensor_msgs::CameraInfoPtr left_cam_info_raw_ms_g( - new sensor_msgs::CameraInfo()); - sensor_msgs::CameraInfoPtr right_cam_info_raw_msg_( - new sensor_msgs::CameraInfo()); + sensor_msgs::CameraInfoPtr rgb_cam_info_raw_msg_(new sensor_msgs::CameraInfo()); + sensor_msgs::CameraInfoPtr left_cam_info_raw_msg_(new sensor_msgs::CameraInfo()); + sensor_msgs::CameraInfoPtr right_cam_info_raw_msg_(new sensor_msgs::CameraInfo()); sensor_msgs::CameraInfoPtr depth_cam_info_msg_(new sensor_msgs::CameraInfo()); - - rgbCamInfoMsg = rgb_cam_info_ms_g; - leftCamInfoMsg = left_cam_info_msg_; - rightCamInfoMsg = right_cam_info_msg_; - rgbCamInfoRawMsg = rgb_cam_info_raw_msg_; - leftCamInfoRawMsg = left_cam_info_raw_ms_g; - rightCamInfoRawMsg = right_cam_info_raw_msg_; - depthCamInfoMsg = depth_cam_info_msg_; + mRgbCamInfoMsg = rgb_cam_info_msg_; + mLeftCamInfoMsg = left_cam_info_msg_; + mRightCamInfoMsg = right_cam_info_msg_; + mRgbCamInfoRawMsg = rgb_cam_info_raw_msg_; + mLeftCamInfoRawMsg = left_cam_info_raw_msg_; + mRightCamInfoRawMsg = right_cam_info_raw_msg_; + mDepthCamInfoMsg = depth_cam_info_msg_; // SVO - nhNs.param("svo_filepath", svoFilepath, std::string()); + mNhNs.param("svo_filepath", mSvoFilepath, std::string()); + + // Initialization transformation listener + mTfBuffer.reset(new tf2_ros::Buffer); + mTfListener.reset(new tf2_ros::TransformListener(*mTfBuffer)); // Initialize tf2 transformation - nhNs.getParam("initial_tracking_pose", initialTrackPose); - set_pose(initialTrackPose[0], initialTrackPose[1], initialTrackPose[2], - initialTrackPose[3], initialTrackPose[4], initialTrackPose[5]); + mNhNs.getParam("initial_tracking_pose", mInitialTrackPose); + set_pose(mInitialTrackPose[0], mInitialTrackPose[1], mInitialTrackPose[2], + mInitialTrackPose[3], mInitialTrackPose[4], mInitialTrackPose[5]); - // Initialization transformation listener - tfBuffer.reset(new tf2_ros::Buffer); - tfListener.reset(new tf2_ros::TransformListener(*tfBuffer)); // Try to initialize the ZED - if (!svoFilepath.empty()) { - param.svo_input_filename = svoFilepath.c_str(); + if (!mSvoFilepath.empty()) { + mZedParams.svo_input_filename = mSvoFilepath.c_str(); + mZedParams.svo_real_time_mode = true; + + // mPubClock = mNh.advertise("/clock", 1); + // NODELET_INFO("Advertised on topic /clock"); + mSvoMode = true; } else { - param.camera_fps = frameRate; - param.camera_resolution = static_cast(resolution); - if (serial_number == 0) { - param.camera_linux_id = zedId; + mZedParams.camera_fps = mCamFrameRate; + mZedParams.camera_resolution = static_cast(mCamResol); + + if (mZedSerialNumber == 0) { + mZedParams.camera_linux_id = mZedId; } else { bool waiting_for_camera = true; - while (waiting_for_camera) { - if (!nhNs.ok()) { + while (waiting_for_camera) { + // Ctrl+C check + if (!mNhNs.ok()) { mStopNode = true; // Stops other threads - zed.close(); + + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); + NODELET_DEBUG("ZED pool thread finished"); return; } - sl::DeviceProperties prop = sl_tools::getZEDFromSN(serial_number); + sl::DeviceProperties prop = sl_tools::getZEDFromSN(mZedSerialNumber); + if (prop.id < -1 || prop.camera_state == sl::CAMERA_STATE::CAMERA_STATE_NOT_AVAILABLE) { - std::string msg = "ZED SN" + to_string(serial_number) + + std::string msg = "ZED SN" + to_string(mZedSerialNumber) + " not detected ! Please connect this ZED"; NODELET_INFO_STREAM(msg.c_str()); std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } else { waiting_for_camera = false; - param.camera_linux_id = prop.id; + mZedParams.camera_linux_id = prop.id; } } } } - std::string ver = sl_tools::getSDKVersion(verMajor, verMinor, verSubMinor); + std::string ver = sl_tools::getSDKVersion(mVerMajor, mVerMinor, mVerSubMinor); NODELET_INFO_STREAM("SDK version : " << ver); - if (verMajor < 2) { - NODELET_WARN_STREAM("Please consider to upgrade to latest SDK version to " - "get better performances"); - param.coordinate_system = COORDINATE_SYSTEM_IMAGE; - NODELET_INFO_STREAM("Camera coordinate system : COORDINATE_SYSTEM_IMAGE"); - xIdx = 2; - yIdx = 0; - zIdx = 1; - xSign = 1; - ySign = -1; - zSign = -1; - } else if (verMajor == 2 && verMinor < 5) { - NODELET_WARN_STREAM("Please consider to upgrade to latest SDK version to " - "get latest features"); - param.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP; - NODELET_INFO_STREAM( - "Camera coordinate system : COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP"); - xIdx = 1; - yIdx = 0; - zIdx = 2; - xSign = 1; - ySign = -1; - zSign = 1; - } else { - param.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD; - NODELET_INFO_STREAM( - "Camera coordinate system : COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD"); - xIdx = 0; - yIdx = 1; - zIdx = 2; - xSign = 1; - ySign = 1; - zSign = 1; - } - - param.coordinate_units = sl::UNIT_METER; +#if (ZED_SDK_MAJOR_VERSION<2) + NODELET_WARN_STREAM("Please consider to upgrade to latest SDK version to " + "get better performances"); + + mZedParams.coordinate_system = sl::COORDINATE_SYSTEM_IMAGE; + + NODELET_INFO_STREAM("Camera coordinate system : COORDINATE_SYSTEM_IMAGE"); + mIdxX = 2; + mIdxY = 0; + mIdxZ = 1; + mSignX = 1; + mSignY = -1; + mSignZ = -1; +#elif (ZED_SDK_MAJOR_VERSION==2 && ZED_SDK_MINOR_VERSION<5) + NODELET_WARN_STREAM("Please consider to upgrade to latest SDK version to " + "get latest features"); + + mZedParams.coordinate_system = sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP; + + NODELET_INFO_STREAM("Camera coordinate system : COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP"); + mIdxX = 1; + mIdxY = 0; + mIdxZ = 2; + mSignX = 1; + mSignY = -1; + mSignZ = 1; +#else + mZedParams.coordinate_system = sl::COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD; + + NODELET_INFO_STREAM("Camera coordinate system : COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD"); + mIdxX = 0; + mIdxY = 1; + mIdxZ = 2; + mSignX = 1; + mSignY = 1; + mSignZ = 1; +#endif - param.depth_mode = static_cast(quality); - param.sdk_verbose = verbose; - param.sdk_gpu_id = gpuId; - param.depth_stabilization = depthStabilization; - param.camera_image_flip = cameraFlip; + mZedParams.coordinate_units = sl::UNIT_METER; + mZedParams.depth_mode = static_cast(mCamQuality); + mZedParams.sdk_verbose = mVerbose; + mZedParams.sdk_gpu_id = mGpuId; + mZedParams.depth_stabilization = mDepthStabilization; + mZedParams.camera_image_flip = mCameraFlip; sl::ERROR_CODE err = sl::ERROR_CODE_CAMERA_NOT_DETECTED; + while (err != sl::SUCCESS) { - err = zed.open(param); + err = mZed.open(mZedParams); NODELET_INFO_STREAM(toString(err)); std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - if (!nhNs.ok()) { + if (!mNhNs.ok()) { mStopNode = true; // Stops other threads - zed.close(); + + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); + NODELET_DEBUG("ZED pool thread finished"); return; } } - realCamModel = zed.getCameraInformation().camera_model; - + mZedRealCamModel = mZed.getCameraInformation().camera_model; std::string camModelStr = "LAST"; - if (realCamModel == sl::MODEL_ZED) { + + if (mZedRealCamModel == sl::MODEL_ZED) { camModelStr = "ZED"; - if (userCamModel != 0) { + + if (mZedUserCamModel != 0) { NODELET_WARN("Camera model does not match user parameter. Please modify " "the value of the parameter 'camera_model' to 0"); } - } else if (realCamModel == sl::MODEL_ZED_M) { + } else if (mZedRealCamModel == sl::MODEL_ZED_M) { camModelStr = "ZED M"; - if (userCamModel != 1) { + + if (mZedUserCamModel != 1) { NODELET_WARN("Camera model does not match user parameter. Please modify " "the value of the parameter 'camera_model' to 1"); } } - NODELET_INFO_STREAM("CAMERA MODEL : " << realCamModel); - - serial_number = zed.getCameraInformation().serial_number; + NODELET_INFO_STREAM("CAMERA MODEL : " << mZedRealCamModel); + mZedSerialNumber = mZed.getCameraInformation().serial_number; // Dynamic Reconfigure parameters - server = - boost::make_shared>(); + mDynRecServer = boost::make_shared>(); dynamic_reconfigure::Server::CallbackType f; f = boost::bind(&ZEDWrapperNodelet::dynamicReconfCallback, this, _1, _2); - server->setCallback(f); + mDynRecServer->setCallback(f); - nhNs.getParam("mat_resize_factor", matResizeFactor); + mNhNs.getParam("mat_resize_factor", mCamMatResizeFactor); - if (matResizeFactor < 0.1) { - matResizeFactor = 0.1; + if (mCamMatResizeFactor < 0.1) { + mCamMatResizeFactor = 0.1; NODELET_WARN_STREAM( "Minimum allowed values for 'mat_resize_factor' is 0.1"); } - if (matResizeFactor > 1.0) { - matResizeFactor = 1.0; + + if (mCamMatResizeFactor > 1.0) { + mCamMatResizeFactor = 1.0; NODELET_WARN_STREAM( "Maximum allowed values for 'mat_resize_factor' is 1.0"); } - nhNs.getParam("confidence", confidence); - nhNs.getParam("exposure", exposure); - nhNs.getParam("gain", gain); - nhNs.getParam("auto_exposure", autoExposure); - if (autoExposure) { - triggerAutoExposure = true; + mNhNs.getParam("confidence", mCamConfidence); + mNhNs.getParam("max_depth", mCamMaxDepth); + mNhNs.getParam("exposure", mCamExposure); + mNhNs.getParam("gain", mCamGain); + mNhNs.getParam("auto_exposure", mCamAutoExposure); + + if (mCamAutoExposure) { + mTriggerAutoExposure = true; } // Create all the publishers // Image publishers - image_transport::ImageTransport it_zed(nh); - pubRgb = it_zed.advertise(rgb_topic, 1); // rgb - NODELET_INFO_STREAM("Advertized on topic " << rgb_topic); - pubRawRgb = it_zed.advertise(rgb_raw_topic, 1); // rgb raw - NODELET_INFO_STREAM("Advertized on topic " << rgb_raw_topic); - pubLeft = it_zed.advertise(left_topic, 1); // left - NODELET_INFO_STREAM("Advertized on topic " << left_topic); - pubRawLeft = it_zed.advertise(left_raw_topic, 1); // left raw - NODELET_INFO_STREAM("Advertized on topic " << left_raw_topic); - pubRight = it_zed.advertise(right_topic, 1); // right - NODELET_INFO_STREAM("Advertized on topic " << right_topic); - pubRawRight = it_zed.advertise(right_raw_topic, 1); // right raw - NODELET_INFO_STREAM("Advertized on topic " << right_raw_topic); - pubDepth = it_zed.advertise(depth_topic, 1); // depth - NODELET_INFO_STREAM("Advertized on topic " << depth_topic); - pubConfImg = it_zed.advertise(conf_img_topic, 1); // confidence image - NODELET_INFO_STREAM("Advertized on topic " << conf_img_topic); + image_transport::ImageTransport it_zed(mNh); + mPubRgb = it_zed.advertise(rgb_topic, 1); // rgb + NODELET_INFO_STREAM("Advertised on topic " << rgb_topic); + mPubRawRgb = it_zed.advertise(rgb_raw_topic, 1); // rgb raw + NODELET_INFO_STREAM("Advertised on topic " << rgb_raw_topic); + mPubLeft = it_zed.advertise(left_topic, 1); // left + NODELET_INFO_STREAM("Advertised on topic " << left_topic); + mPubRawLeft = it_zed.advertise(left_raw_topic, 1); // left raw + NODELET_INFO_STREAM("Advertised on topic " << left_raw_topic); + mPubRight = it_zed.advertise(right_topic, 1); // right + NODELET_INFO_STREAM("Advertised on topic " << right_topic); + mPubRawRight = it_zed.advertise(right_raw_topic, 1); // right raw + NODELET_INFO_STREAM("Advertised on topic " << right_raw_topic); + mPubDepth = it_zed.advertise(depth_topic, 1); // depth + NODELET_INFO_STREAM("Advertised on topic " << depth_topic); + mPubConfImg = it_zed.advertise(conf_img_topic, 1); // confidence image + NODELET_INFO_STREAM("Advertised on topic " << conf_img_topic); // Confidence Map publisher - pubConfMap = - nh.advertise(conf_map_topic, 1); // confidence map - NODELET_INFO_STREAM("Advertized on topic " << conf_map_topic); + mPubConfMap = mNh.advertise(conf_map_topic, 1); // confidence map + NODELET_INFO_STREAM("Advertised on topic " << conf_map_topic); // Disparity publisher - pubDisparity = nh.advertise(disparity_topic, 1); - NODELET_INFO_STREAM("Advertized on topic " << disparity_topic); + mPubDisparity = mNh.advertise(disparity_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << disparity_topic); // PointCloud publisher - pubCloud = nh.advertise(point_cloud_topic, 1); - NODELET_INFO_STREAM("Advertized on topic " << point_cloud_topic); + mPubCloud = mNh.advertise(point_cloud_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << point_cloud_topic); // Camera info publishers - pubRgbCamInfo = nh.advertise(rgb_cam_info_topic, 1); // rgb - NODELET_INFO_STREAM("Advertized on topic " << rgb_cam_info_topic); - pubLeftCamInfo = nh.advertise(left_cam_info_topic, 1); // left - NODELET_INFO_STREAM("Advertized on topic " << left_cam_info_topic); - pubRightCamInfo = nh.advertise(right_cam_info_topic, 1); // right - NODELET_INFO_STREAM("Advertized on topic " << right_cam_info_topic); - pubDepthCamInfo = nh.advertise(depth_cam_info_topic, 1); // depth - NODELET_INFO_STREAM("Advertized on topic " << depth_cam_info_topic); + mPubRgbCamInfo = mNh.advertise(rgb_cam_info_topic, 1); // rgb + NODELET_INFO_STREAM("Advertised on topic " << rgb_cam_info_topic); + mPubLeftCamInfo = mNh.advertise(left_cam_info_topic, 1); // left + NODELET_INFO_STREAM("Advertised on topic " << left_cam_info_topic); + mPubRightCamInfo = mNh.advertise(right_cam_info_topic, 1); // right + NODELET_INFO_STREAM("Advertised on topic " << right_cam_info_topic); + mPubDepthCamInfo = mNh.advertise(depth_cam_info_topic, 1); // depth + NODELET_INFO_STREAM("Advertised on topic " << depth_cam_info_topic); + // Raw - pubRgbCamInfoRaw = nh.advertise(rgb_cam_info_raw_topic, 1); // raw rgb - NODELET_INFO_STREAM("Advertized on topic " << rgb_cam_info_raw_topic); - pubLeftCamInfoRaw = nh.advertise(left_cam_info_raw_topic, 1); // raw left - NODELET_INFO_STREAM("Advertized on topic " << left_cam_info_raw_topic); - pubRightCamInfoRaw = nh.advertise(right_cam_info_raw_topic, 1); // raw right - NODELET_INFO_STREAM("Advertized on topic " << right_cam_info_raw_topic); + mPubRgbCamInfoRaw = mNh.advertise(rgb_cam_info_raw_topic, 1); // raw rgb + NODELET_INFO_STREAM("Advertised on topic " << rgb_cam_info_raw_topic); + mPubLeftCamInfoRaw = mNh.advertise(left_cam_info_raw_topic, 1); // raw left + NODELET_INFO_STREAM("Advertised on topic " << left_cam_info_raw_topic); + mPubRightCamInfoRaw = mNh.advertise(right_cam_info_raw_topic, 1); // raw right + NODELET_INFO_STREAM("Advertised on topic " << right_cam_info_raw_topic); // Odometry and Map publisher - pubPose = nh.advertise(pose_topic, 1); - NODELET_INFO_STREAM("Advertized on topic " << pose_topic); - pubOdom = nh.advertise(odometry_topic, 1); - NODELET_INFO_STREAM("Advertized on topic " << odometry_topic); + mPubPose = mNh.advertise(pose_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << pose_topic); + if (mPublishPoseCovariance) { + mPubPoseCov = mNh.advertise(pose_cov_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << pose_cov_topic); + } + mPubOdom = mNh.advertise(odometry_topic, 1); + NODELET_INFO_STREAM("Advertised on topic " << odometry_topic); + + // Camera Path + if (mPathPubRate > 0) { + mPubOdomPath = mNh.advertise(odom_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << odom_path_topic); + mPubMapPath = mNh.advertise(map_path_topic, 1, true); + NODELET_INFO_STREAM("Advertised on topic " << map_path_topic); + + mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), + &ZEDWrapperNodelet::pathPubCallback, this); + + if (mPathMaxCount != -1) { + NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); + mOdomPath.reserve(mPathMaxCount); + mMapPath.reserve(mPathMaxCount); + + NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size()); + } + } // Imu publisher - if (imuPubRate > 0 && realCamModel == sl::MODEL_ZED_M) { - pubImu = nh.advertise(imu_topic, 500); - NODELET_INFO_STREAM("Advertized on topic " << imu_topic << " @ " << imuPubRate << " Hz"); - - pubImuRaw = nh.advertise(imu_topic_raw, 500); - NODELET_INFO_STREAM("Advertized on topic " << imu_topic_raw << " @ " << imuPubRate << " Hz"); - - imuTime = ros::Time::now(); - pubImuTimer = nhNs.createTimer(ros::Duration(1.0 / imuPubRate), - &ZEDWrapperNodelet::imuPubCallback, this); - } else if (imuPubRate > 0 && realCamModel == sl::MODEL_ZED) { + if (mImuPubRate > 0 && mZedRealCamModel == sl::MODEL_ZED_M) { + mPubImu = mNh.advertise(imu_topic, 500); + NODELET_INFO_STREAM("Advertised on topic " << imu_topic << " @ " + << mImuPubRate << " Hz"); + mPubImuRaw = mNh.advertise(imu_topic_raw, 500); + NODELET_INFO_STREAM("Advertised on topic " << imu_topic_raw << " @ " + << mImuPubRate << " Hz"); + mFrameTimestamp = ros::Time::now(); + mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / mImuPubRate), + &ZEDWrapperNodelet::imuPubCallback, this); + } else if (mImuPubRate > 0 && mZedRealCamModel == sl::MODEL_ZED) { NODELET_WARN_STREAM( "'imu_pub_rate' set to " - << imuPubRate << " Hz" + << mImuPubRate << " Hz" << " but ZED camera model does not support IMU data publishing."); } - - // Service - srvSetInitPose = nh.advertiseService("set_initial_pose", &ZEDWrapperNodelet::on_set_pose, this); - srvResetOdometry = nh.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); - srvResetTracking = nh.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + + // Services + mSrvSetInitPose = mNh.advertiseService("set_initial_pose", &ZEDWrapperNodelet::on_set_pose, this); + mSrvResetOdometry = mNh.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); + mSrvResetTracking = mNh.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); // Start Pointcloud thread - mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread, this); + mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread_func, this); // Start pool thread - devicePollThread = std::thread(&ZEDWrapperNodelet::device_poll, this); + mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); } void ZEDWrapperNodelet::checkResolFps() { - switch (resolution) { + switch (mCamResol) { case sl::RESOLUTION_HD2K: - if (frameRate != 15) { + if (mCamFrameRate != 15) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD2K. Set to 15 FPS."); - frameRate = 15; + mCamFrameRate = 15; } + break; case sl::RESOLUTION_HD1080: - if (frameRate == 15 || frameRate == 30) { + if (mCamFrameRate == 15 || mCamFrameRate == 30) { break; } - if (frameRate > 15 && frameRate < 30) { + if (mCamFrameRate > 15 && mCamFrameRate < 30) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - frameRate = 15; - } else if (frameRate > 30) { + mCamFrameRate = 15; + } else if (mCamFrameRate > 30) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD1080. Set to 30 FPS."); - frameRate = 30; + mCamFrameRate = 30; } else { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD1080. Set to 15 FPS."); - frameRate = 15; + mCamFrameRate = 15; } + break; case sl::RESOLUTION_HD720: - if (frameRate == 15 || frameRate == 30 || frameRate == 60) { + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60) { break; } - if (frameRate > 15 && frameRate < 30) { + if (mCamFrameRate > 15 && mCamFrameRate < 30) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); - frameRate = 15; - } else if (frameRate > 30 && frameRate < 60) { + mCamFrameRate = 15; + } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD720. Set to 30 FPS."); - frameRate = 30; - } else if (frameRate > 60) { + mCamFrameRate = 30; + } else if (mCamFrameRate > 60) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD720. Set to 60 FPS."); - frameRate = 60; + mCamFrameRate = 60; } else { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution HD720. Set to 15 FPS."); - frameRate = 15; + mCamFrameRate = 15; } + break; case sl::RESOLUTION_VGA: - if (frameRate == 15 || frameRate == 30 || frameRate == 60 || - frameRate == 100) { + if (mCamFrameRate == 15 || mCamFrameRate == 30 || mCamFrameRate == 60 || + mCamFrameRate == 100) { break; } - if (frameRate > 15 && frameRate < 30) { + if (mCamFrameRate > 15 && mCamFrameRate < 30) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); - frameRate = 15; - } else if (frameRate > 30 && frameRate < 60) { + mCamFrameRate = 15; + } else if (mCamFrameRate > 30 && mCamFrameRate < 60) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution VGA. Set to 30 FPS."); - frameRate = 30; - } else if (frameRate > 60 && frameRate < 100) { + mCamFrameRate = 30; + } else if (mCamFrameRate > 60 && mCamFrameRate < 100) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution VGA. Set to 60 FPS."); - frameRate = 60; - } else if (frameRate > 100) { + mCamFrameRate = 60; + } else if (mCamFrameRate > 100) { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution VGA. Set to 100 FPS."); - frameRate = 100; + mCamFrameRate = 100; } else { NODELET_WARN_STREAM("Wrong FrameRate (" - << frameRate + << mCamFrameRate << ") for the resolution VGA. Set to 15 FPS."); - frameRate = 15; + mCamFrameRate = 15; } + break; default: NODELET_WARN_STREAM("Invalid resolution. Set to HD720 @ 30 FPS"); - resolution = 2; - frameRate = 30; + mCamResol = 2; + mCamFrameRate = 30; } } - sensor_msgs::ImagePtr ZEDWrapperNodelet::imageToROSmsg(sl::Mat img, std::string frameId, ros::Time t) { - - sensor_msgs::ImagePtr ptr = boost::make_shared(); - sensor_msgs::Image& imgMessage = *ptr; - - imgMessage.header.stamp = t; - imgMessage.header.frame_id = frameId; - imgMessage.height = img.getHeight(); - imgMessage.width = img.getWidth(); - - int num = 1; // for endianness detection - imgMessage.is_bigendian = !(*(char*)&num == 1); - - imgMessage.step = img.getStepBytes(); - - size_t size = imgMessage.step * imgMessage.height; - imgMessage.data.resize(size); - - sl::MAT_TYPE dataType = img.getDataType(); - - switch (dataType) { - case sl::MAT_TYPE_32F_C1: /**< float 1 channel.*/ - imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; - case sl::MAT_TYPE_32F_C2: /**< float 2 channels.*/ - imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC2; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; - case sl::MAT_TYPE_32F_C3: /**< float 3 channels.*/ - imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC3; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; - case sl::MAT_TYPE_32F_C4: /**< float 4 channels.*/ - imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC4; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; - case sl::MAT_TYPE_8U_C1: /**< unsigned char 1 channel.*/ - imgMessage.encoding = sensor_msgs::image_encodings::MONO8; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; - case sl::MAT_TYPE_8U_C2: /**< unsigned char 2 channels.*/ - imgMessage.encoding = sensor_msgs::image_encodings::TYPE_8UC2; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; - case sl::MAT_TYPE_8U_C3: /**< unsigned char 3 channels.*/ - imgMessage.encoding = sensor_msgs::image_encodings::BGR8; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; - case sl::MAT_TYPE_8U_C4: /**< unsigned char 4 channels.*/ - imgMessage.encoding = sensor_msgs::image_encodings::BGRA8; - memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); - break; + void ZEDWrapperNodelet::initTransforms() { + // >>>>> Dynamic transforms + mBase2OdomTransf.setIdentity(); + mOdom2MapTransf.setIdentity(); + // <<<<< Dynamic transforms + + // >>>>> Static transforms + // Sensor to Base link + try { + // Save the transformation from base to frame + geometry_msgs::TransformStamped s2b = + mTfBuffer->lookupTransform(mBaseFrameId, mDepthFrameId, mFrameTimestamp); + // Get the TF2 transformation + tf2::fromMsg(s2b.transform, mSensor2BaseTransf); + } catch (tf2::TransformException& ex) { + NODELET_WARN_THROTTLE( + 10.0, "The tf from '%s' to '%s' does not seem to be available, " + "will assume it as identity!", + mDepthFrameId.c_str(), mBaseFrameId.c_str()); + NODELET_DEBUG("Transform error: %s", ex.what()); + mSensor2BaseTransf.setIdentity(); } + // <<<<< Static transforms - - - return ptr; } void ZEDWrapperNodelet::set_pose(float xt, float yt, float zt, float rr, @@ -672,13 +674,12 @@ namespace zed_wrapper { // ROS pose tf2::Quaternion q; q.setRPY(rr, pr, yr); + //tf2::Vector3 orig(xt, yt, zt); + // mBase2OdomTransf.setOrigin(orig); + // mBase2OdomTransf.setRotation(q); + // mOdom2MapTransf.setIdentity(); - tf2::Vector3 orig(xt, yt, zt); - - baseToOdomTransform.setOrigin(orig); - baseToOdomTransform.setRotation(q); - - odomToMapTransform.setIdentity(); + initTransforms(); // SL pose sl::float4 q_vec; @@ -686,201 +687,277 @@ namespace zed_wrapper { q_vec[1] = q.y(); q_vec[2] = q.z(); q_vec[3] = q.w(); - sl::Orientation r(q_vec); - - initialPoseSl.setTranslation(sl::Translation(xt, yt, zt)); - initialPoseSl.setOrientation(r); + mInitialPoseSl.setTranslation(sl::Translation(xt, yt, zt)); + mInitialPoseSl.setOrientation(r); } bool ZEDWrapperNodelet::on_set_pose( zed_wrapper::set_initial_pose::Request& req, zed_wrapper::set_initial_pose::Response& res) { - initialTrackPose.resize(6); - - initialTrackPose[0] = req.x; - initialTrackPose[1] = req.y; - initialTrackPose[2] = req.z; - initialTrackPose[3] = req.R; - initialTrackPose[4] = req.P; - initialTrackPose[5] = req.Y; - - set_pose(initialTrackPose[0], initialTrackPose[1], initialTrackPose[2], - initialTrackPose[3], initialTrackPose[4], initialTrackPose[5]); - - if (trackingActivated) { - zed.resetTracking(initialPoseSl); + mInitialTrackPose.resize(6); + mInitialTrackPose[0] = req.x; + mInitialTrackPose[1] = req.y; + mInitialTrackPose[2] = req.z; + mInitialTrackPose[3] = req.R; + mInitialTrackPose[4] = req.P; + mInitialTrackPose[5] = req.Y; + set_pose(mInitialTrackPose[0], mInitialTrackPose[1], mInitialTrackPose[2], + mInitialTrackPose[3], mInitialTrackPose[4], mInitialTrackPose[5]); + + if (mTrackingActivated) { + mZed.resetTracking(mInitialPoseSl); } res.done = true; - return true; } bool ZEDWrapperNodelet::on_reset_tracking( zed_wrapper::reset_tracking::Request& req, zed_wrapper::reset_tracking::Response& res) { - if (!trackingActivated) { + if (!mTrackingActivated) { res.reset_done = false; return false; } - nhNs.getParam("initial_tracking_pose", initialTrackPose); + mNhNs.getParam("initial_tracking_pose", mInitialTrackPose); - if (initialTrackPose.size() != 6) { - NODELET_WARN_STREAM("Invalid Initial Pose size (" << initialTrackPose.size() + if (mInitialTrackPose.size() != 6) { + NODELET_WARN_STREAM("Invalid Initial Pose size (" << mInitialTrackPose.size() << "). Using Identity"); - initialPoseSl.setIdentity(); - odomToMapTransform.setIdentity(); - baseToOdomTransform.setIdentity(); + mInitialPoseSl.setIdentity(); + + initTransforms(); } else { - set_pose(initialTrackPose[0], initialTrackPose[1], initialTrackPose[2], - initialTrackPose[3], initialTrackPose[4], initialTrackPose[5]); + set_pose(mInitialTrackPose[0], mInitialTrackPose[1], mInitialTrackPose[2], + mInitialTrackPose[3], mInitialTrackPose[4], mInitialTrackPose[5]); } - zed.resetTracking(initialPoseSl); + if (mZed.resetTracking(mInitialPoseSl) == sl::SUCCESS) { + return true; + } - return true; + return false; } bool ZEDWrapperNodelet::on_reset_odometry( zed_wrapper::reset_odometry::Request& req, zed_wrapper::reset_odometry::Response& res) { - initOdomWithPose = true; - + mResetOdom = true; res.reset_done = true; - return true; } void ZEDWrapperNodelet::start_tracking() { NODELET_INFO_STREAM("Starting Tracking"); - - nhNs.getParam("odometry_DB", odometryDb); - nhNs.getParam("pose_smoothing", poseSmoothing); - NODELET_INFO_STREAM("Pose Smoothing : " << poseSmoothing); - nhNs.getParam("spatial_memory", spatialMemory); - NODELET_INFO_STREAM("Spatial Memory : " << spatialMemory); - if (realCamModel == sl::MODEL_ZED_M) { - nhNs.getParam("init_odom_with_imu", initOdomWithPose); - NODELET_INFO_STREAM( - "Init Odometry with first IMU data : " << initOdomWithPose); - } else { - initOdomWithPose = false; - } - - if (initialTrackPose.size() != 6) { - NODELET_WARN_STREAM("Invalid Initial Pose size (" << initialTrackPose.size() + mNhNs.getParam("odometry_DB", mOdometryDb); + mNhNs.getParam("pose_smoothing", mPoseSmoothing); + mNhNs.getParam("spatial_memory", mSpatialMemory); + mNhNs.getParam("floor_alignment", mFloorAlignment); + mNhNs.getParam("init_odom_with_first_valid_pose", mInitOdomWithPose); + NODELET_INFO_STREAM("Init Odometry with first valid pose data : " << mInitOdomWithPose); + + if (mInitialTrackPose.size() != 6) { + NODELET_WARN_STREAM("Invalid Initial Pose size (" << mInitialTrackPose.size() << "). Using Identity"); - initialPoseSl.setIdentity(); - odomToMapTransform.setIdentity(); - odomToMapTransform.setIdentity(); + mInitialPoseSl.setIdentity(); + + initTransforms(); } else { - set_pose(initialTrackPose[0], initialTrackPose[1], initialTrackPose[2], - initialTrackPose[3], initialTrackPose[4], initialTrackPose[5]); + set_pose(mInitialTrackPose[0], mInitialTrackPose[1], mInitialTrackPose[2], + mInitialTrackPose[3], mInitialTrackPose[4], mInitialTrackPose[5]); } - if (odometryDb != "" && !sl_tools::file_exist(odometryDb)) { - odometryDb = ""; + if (mOdometryDb != "" && !sl_tools::file_exist(mOdometryDb)) { + mOdometryDb = ""; NODELET_WARN("odometry_DB path doesn't exist or is unreachable."); } // Tracking parameters sl::TrackingParameters trackParams; - trackParams.area_file_path = odometryDb.c_str(); - trackParams.enable_pose_smoothing = poseSmoothing; - trackParams.enable_spatial_memory = spatialMemory; - - trackParams.initial_world_transform = initialPoseSl; + trackParams.area_file_path = mOdometryDb.c_str(); + trackParams.enable_pose_smoothing = mPoseSmoothing; + NODELET_INFO_STREAM("Pose Smoothing : " << trackParams.enable_pose_smoothing); + trackParams.enable_spatial_memory = mSpatialMemory; + NODELET_INFO_STREAM("Spatial Memory : " << trackParams.enable_spatial_memory); + trackParams.initial_world_transform = mInitialPoseSl; + + if (mFloorAlignment && + ((ZED_SDK_MAJOR_VERSION < 2) || (ZED_SDK_MAJOR_VERSION == 2 && ZED_SDK_MINOR_VERSION < 6))) { + NODELET_WARN("Floor Alignment is available starting from SDK v2.6"); + } else { + trackParams.set_floor_as_origin = mFloorAlignment; + NODELET_INFO_STREAM("Floor Alignment : " << trackParams.set_floor_as_origin); + } - zed.enableTracking(trackParams); - trackingActivated = true; + mZed.enableTracking(trackParams); + mTrackingActivated = true; + NODELET_INFO("Tracking ENABLED"); } - void ZEDWrapperNodelet::publishOdom(tf2::Transform odom_base_transform, - ros::Time t) { + void ZEDWrapperNodelet::publishOdom(tf2::Transform base2odomTransf, sl::Pose& slPose, ros::Time t) { nav_msgs::Odometry odom; odom.header.stamp = t; - odom.header.frame_id = odometryFrameId; // odom_frame - odom.child_frame_id = baseFrameId; // base_frame + odom.header.frame_id = mPublishMapTf ? mMapFrameId : mOdometryFrameId; // frame + odom.child_frame_id = mBaseFrameId; // camera_frame // conversion from Tranform to message - geometry_msgs::Transform base2 = tf2::toMsg(odom_base_transform); + geometry_msgs::Transform base2odom = tf2::toMsg(base2odomTransf); // Add all value in odometry message - odom.pose.pose.position.x = base2.translation.x; - odom.pose.pose.position.y = base2.translation.y; - odom.pose.pose.position.z = base2.translation.z; - odom.pose.pose.orientation.x = base2.rotation.x; - odom.pose.pose.orientation.y = base2.rotation.y; - odom.pose.pose.orientation.z = base2.rotation.z; - odom.pose.pose.orientation.w = base2.rotation.w; + odom.pose.pose.position.x = base2odom.translation.x; + odom.pose.pose.position.y = base2odom.translation.y; + odom.pose.pose.position.z = base2odom.translation.z; + odom.pose.pose.orientation.x = base2odom.rotation.x; + odom.pose.pose.orientation.y = base2odom.rotation.y; + odom.pose.pose.orientation.z = base2odom.rotation.z; + odom.pose.pose.orientation.w = base2odom.rotation.w; + + // >>>>> Odometry pose covariance if available +#if ((ZED_SDK_MAJOR_VERSION>2) || (ZED_SDK_MAJOR_VERSION==2 && ZED_SDK_MINOR_VERSION>=6)) + if (!mSpatialMemory && mPublishPoseCovariance) { + for (size_t i = 0; i < odom.pose.covariance.size(); i++) { + // odom.pose.covariance[i] = static_cast(slPose.pose_covariance[i]); // TODO USE THIS WHEN STEP BY STEP COVARIANCE WILL BE AVAILABLE IN CAMERA_FRAME + odom.pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); + } + } +#endif + // <<<<< Odometry pose covariance if available + // Publish odometry message - pubOdom.publish(odom); + mPubOdom.publish(odom); } - void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform baseTransform, - ros::Time t) { - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = t; - transformStamped.header.frame_id = odometryFrameId; - transformStamped.child_frame_id = baseFrameId; + void ZEDWrapperNodelet::publishPose(ros::Time t) { + tf2::Transform base_pose; + base_pose.setIdentity(); + + if (mPublishMapTf) { + // Look up the transformation from base frame to map + try { + // Save the transformation from base to frame + geometry_msgs::TransformStamped b2m = + mTfBuffer->lookupTransform(mMapFrameId, mBaseFrameId, ros::Time(0)); + // Get the TF2 transformation + tf2::fromMsg(b2m.transform, base_pose); + } catch (tf2::TransformException& ex) { + NODELET_WARN_THROTTLE( + 10.0, "The tf from '%s' to '%s' does not seem to be available, " + "will assume it as identity!", + mBaseFrameId.c_str(), mMapFrameId.c_str()); + NODELET_DEBUG("Transform error: %s", ex.what()); + } + } else { + // Look up the transformation from base frame to odom frame + try { + // Save the transformation from base to frame + geometry_msgs::TransformStamped b2o = + mTfBuffer->lookupTransform(mOdometryFrameId, mBaseFrameId, ros::Time(0)); + // Get the TF2 transformation + tf2::fromMsg(b2o.transform, base_pose); + } catch (tf2::TransformException& ex) { + NODELET_WARN_THROTTLE( + 10.0, "The tf from '%s' to '%s' does not seem to be available, " + "will assume it as identity!", + mBaseFrameId.c_str(), mOdometryFrameId.c_str()); + NODELET_DEBUG("Transform error: %s", ex.what()); + } + } + + std_msgs::Header header; + header.stamp = mFrameTimestamp; + header.frame_id = mPublishMapTf ? mMapFrameId : mOdometryFrameId; // frame + // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(baseTransform); - // Publish transformation - transformOdomBroadcaster.sendTransform(transformStamped); + geometry_msgs::Transform base2frame = tf2::toMsg(base_pose); + + // Add all value in Pose message + geometry_msgs::Pose pose; + pose.position.x = base2frame.translation.x; + pose.position.y = base2frame.translation.y; + pose.position.z = base2frame.translation.z; + pose.orientation.x = base2frame.rotation.x; + pose.orientation.y = base2frame.rotation.y; + pose.orientation.z = base2frame.rotation.z; + pose.orientation.w = base2frame.rotation.w; + + if (mPubPose.getNumSubscribers() > 0) { + + geometry_msgs::PoseStamped poseNoCov; + + poseNoCov.header = header; + poseNoCov.pose = pose; + + // Publish pose stamped message + mPubPose.publish(poseNoCov); + } + + if (mPublishPoseCovariance) { + if (mPubPoseCov.getNumSubscribers() > 0) { + geometry_msgs::PoseWithCovarianceStamped poseCov; + + poseCov.header = header; + poseCov.pose.pose = pose; + + // >>>>> Odometry pose covariance if available +#if ((ZED_SDK_MAJOR_VERSION>2) || (ZED_SDK_MAJOR_VERSION==2 && ZED_SDK_MINOR_VERSION>=6)) + if (!mSpatialMemory) { + for (size_t i = 0; i < poseCov.pose.covariance.size(); i++) { + // odom.pose.covariance[i] = static_cast(slPose.pose_covariance[i]); // TODO USE THIS WHEN STEP BY STEP COVARIANCE WILL BE AVAILABLE IN CAMERA_FRAME + poseCov.pose.covariance[i] = static_cast(mLastZedPose.pose_covariance[i]); + } + } +#endif + // <<<<< Odometry pose covariance if available + + // Publish pose with covariance stamped message + mPubPoseCov.publish(poseCov); + } + } } - void ZEDWrapperNodelet::publishPose(tf2::Transform poseBaseTransform, - ros::Time t) { - geometry_msgs::PoseStamped pose; - pose.header.stamp = t; - pose.header.frame_id = mapFrameId; // map_frame + void ZEDWrapperNodelet::publishOdomFrame(tf2::Transform odomTransf, ros::Time t) { + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mOdometryFrameId; + transformStamped.child_frame_id = mBaseFrameId; // conversion from Tranform to message - geometry_msgs::Transform base2 = tf2::toMsg(poseBaseTransform); - // Add all value in Pose message - pose.pose.position.x = base2.translation.x; - pose.pose.position.y = base2.translation.y; - pose.pose.position.z = base2.translation.z; - pose.pose.orientation.x = base2.rotation.x; - pose.pose.orientation.y = base2.rotation.y; - pose.pose.orientation.z = base2.rotation.z; - pose.pose.orientation.w = base2.rotation.w; - // Publish odometry message - pubPose.publish(pose); + transformStamped.transform = tf2::toMsg(odomTransf); + // Publish transformation + mTransformOdomBroadcaster.sendTransform(transformStamped); } - void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, - ros::Time t) { + void ZEDWrapperNodelet::publishPoseFrame(tf2::Transform baseTransform, ros::Time t) { geometry_msgs::TransformStamped transformStamped; transformStamped.header.stamp = t; - transformStamped.header.frame_id = mapFrameId; - transformStamped.child_frame_id = odometryFrameId; + transformStamped.header.frame_id = mMapFrameId; + transformStamped.child_frame_id = mOdometryFrameId; // conversion from Tranform to message transformStamped.transform = tf2::toMsg(baseTransform); // Publish transformation - transformPoseBroadcaster.sendTransform(transformStamped); + mTransformPoseBroadcaster.sendTransform(transformStamped); } - void ZEDWrapperNodelet::publishImuFrame(tf2::Transform baseTransform) { + void ZEDWrapperNodelet::publishImuFrame(tf2::Transform imuTransform, ros::Time t) { geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = imuTime; - transformStamped.header.frame_id = baseFrameId; - transformStamped.child_frame_id = imuFrameId; + transformStamped.header.stamp = t; + transformStamped.header.frame_id = mCameraFrameId; + transformStamped.child_frame_id = mImuFrameId; // conversion from Tranform to message - transformStamped.transform = tf2::toMsg(baseTransform); + transformStamped.transform = tf2::toMsg(imuTransform); // Publish transformation - transformImuBroadcaster.sendTransform(transformStamped); + mTransformImuBroadcaster.sendTransform(transformStamped); } void ZEDWrapperNodelet::publishImage(sl::Mat img, image_transport::Publisher& pubImg, string imgFrameId, ros::Time t) { - pubImg.publish(imageToROSmsg(img, imgFrameId, t)); + pubImg.publish(sl_tools::imageToROSmsg(img, imgFrameId, t)); } void ZEDWrapperNodelet::publishDepth(sl::Mat depth, ros::Time t) { - if (!openniDepthMode) { - pubDepth.publish(imageToROSmsg(depth, depthOptFrameId, t)); + if (!mOpenniDepthMode) { + mPubDepth.publish(sl_tools::imageToROSmsg(depth, mDepthOptFrameId, t)); return; } @@ -888,7 +965,7 @@ namespace zed_wrapper { sensor_msgs::ImagePtr depthMessage = boost::make_shared(); depthMessage->header.stamp = t; - depthMessage->header.frame_id = depthOptFrameId; + depthMessage->header.frame_id = mDepthOptFrameId; depthMessage->height = depth.getHeight(); depthMessage->width = depth.getWidth(); @@ -910,27 +987,27 @@ namespace zed_wrapper { *(data++) = static_cast(std::round(*(depthDataPtr++) * 1000)); // in mm, rounded } - pubDepth.publish(depthMessage); + mPubDepth.publish(depthMessage); } void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) { sl::CameraInformation zedParam = - zed.getCameraInformation(sl::Resolution(matWidth, matHeight)); + mZed.getCameraInformation(sl::Resolution(mMatWidth, mMatHeight)); - sensor_msgs::ImagePtr disparity_image = imageToROSmsg(disparity, disparityFrameId, t); + sensor_msgs::ImagePtr disparity_image = sl_tools::imageToROSmsg(disparity, mDisparityFrameId, t); stereo_msgs::DisparityImage msg; msg.image = *disparity_image; msg.header = msg.image.header; msg.f = zedParam.calibration_parameters.left_cam.fx; msg.T = zedParam.calibration_parameters.T.x; - msg.min_disparity = msg.f * msg.T / zed.getDepthMaxRangeValue(); - msg.max_disparity = msg.f * msg.T / zed.getDepthMinRangeValue(); - pubDisparity.publish(msg); + msg.min_disparity = msg.f * msg.T / mZed.getDepthMaxRangeValue(); + msg.max_disparity = msg.f * msg.T / mZed.getDepthMinRangeValue(); + mPubDisparity.publish(msg); } - void ZEDWrapperNodelet::pointcloud_thread() { + void ZEDWrapperNodelet::pointcloud_thread_func() { std::unique_lock lock(mPcMutex); while (!mStopNode) { while (!mPcDataReady) { // loop to avoid spurious wakeups @@ -954,17 +1031,17 @@ namespace zed_wrapper { void ZEDWrapperNodelet::publishPointCloud() { // Initialize Point Cloud message // https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h - int ptsCount = matWidth * matHeight; - mPointcloudMsg.header.stamp = pointCloudTime; - if (mPointcloudMsg.width != matWidth || mPointcloudMsg.height != matHeight) { - mPointcloudMsg.header.frame_id = pointCloudFrameId; // Set the header values of the ROS message + int ptsCount = mMatWidth * mMatHeight; + mPointcloudMsg.header.stamp = mPointCloudTime; + if (mPointcloudMsg.width != mMatWidth || mPointcloudMsg.height != mMatHeight) { + mPointcloudMsg.header.frame_id = mPointCloudFrameId; // Set the header values of the ROS message mPointcloudMsg.is_bigendian = false; mPointcloudMsg.is_dense = false; - mPointcloudMsg.width = matWidth; - mPointcloudMsg.height = matHeight; + mPointcloudMsg.width = mMatWidth; + mPointcloudMsg.height = mMatHeight; sensor_msgs::PointCloud2Modifier modifier(mPointcloudMsg); modifier.setPointCloud2Fields(4, @@ -974,21 +1051,21 @@ namespace zed_wrapper { "rgb", 1, sensor_msgs::PointField::FLOAT32); } - sl::Vector4* cpu_cloud = cloud.getPtr(); + sl::Vector4* cpu_cloud = mCloud.getPtr(); // Data copy float* ptCloudPtr = (float*)(&mPointcloudMsg.data[0]); #pragma omp parallel for for (size_t i = 0; i < ptsCount; ++i) { - ptCloudPtr[i * 4 + 0] = xSign * cpu_cloud[i][xIdx]; - ptCloudPtr[i * 4 + 1] = ySign * cpu_cloud[i][yIdx]; - ptCloudPtr[i * 4 + 2] = zSign * cpu_cloud[i][zIdx]; + ptCloudPtr[i * 4 + 0] = mSignX * cpu_cloud[i][mIdxX]; + ptCloudPtr[i * 4 + 1] = mSignY * cpu_cloud[i][mIdxY]; + ptCloudPtr[i * 4 + 2] = mSignZ * cpu_cloud[i][mIdxZ]; ptCloudPtr[i * 4 + 3] = cpu_cloud[i][3]; } // Pointcloud publishing - pubCloud.publish(mPointcloudMsg); + mPubCloud.publish(mPointcloudMsg); } void ZEDWrapperNodelet::publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, @@ -1000,552 +1077,685 @@ namespace zed_wrapper { seq++; } - void ZEDWrapperNodelet::fillCamInfo( - sl::Camera& zed, sensor_msgs::CameraInfoPtr left_cam_info_msg, - sensor_msgs::CameraInfoPtr right_cam_info_msg, string leftFrameId, - string rightFrameId, bool rawParam /*= false*/) { + void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr leftCamInfoMsg, + sensor_msgs::CameraInfoPtr rightCamInfoMsg, string leftFrameId, + string rightFrameId, bool rawParam /*= false*/) { sl::CalibrationParameters zedParam; - if (rawParam) - zedParam = zed.getCameraInformation(sl::Resolution(matWidth, matHeight)) + if (rawParam) { + zedParam = zed.getCameraInformation(sl::Resolution(mMatWidth, mMatHeight)) .calibration_parameters_raw; - else - zedParam = zed.getCameraInformation(sl::Resolution(matWidth, matHeight)) + } else { + zedParam = zed.getCameraInformation(sl::Resolution(mMatWidth, mMatHeight)) .calibration_parameters; + } float baseline = zedParam.T[0]; - - left_cam_info_msg->distortion_model = + leftCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - right_cam_info_msg->distortion_model = + rightCamInfoMsg->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; - - left_cam_info_msg->D.resize(5); - right_cam_info_msg->D.resize(5); - left_cam_info_msg->D[0] = zedParam.left_cam.disto[0]; // k1 - left_cam_info_msg->D[1] = zedParam.left_cam.disto[1]; // k2 - left_cam_info_msg->D[2] = zedParam.left_cam.disto[4]; // k3 - left_cam_info_msg->D[3] = zedParam.left_cam.disto[2]; // p1 - left_cam_info_msg->D[4] = zedParam.left_cam.disto[3]; // p2 - right_cam_info_msg->D[0] = zedParam.right_cam.disto[0]; // k1 - right_cam_info_msg->D[1] = zedParam.right_cam.disto[1]; // k2 - right_cam_info_msg->D[2] = zedParam.right_cam.disto[4]; // k3 - right_cam_info_msg->D[3] = zedParam.right_cam.disto[2]; // p1 - right_cam_info_msg->D[4] = zedParam.right_cam.disto[3]; // p2 - - left_cam_info_msg->K.fill(0.0); - right_cam_info_msg->K.fill(0.0); - left_cam_info_msg->K[0] = zedParam.left_cam.fx; - left_cam_info_msg->K[2] = zedParam.left_cam.cx; - left_cam_info_msg->K[4] = zedParam.left_cam.fy; - left_cam_info_msg->K[5] = zedParam.left_cam.cy; - left_cam_info_msg->K[8] = 1.0; - right_cam_info_msg->K[0] = zedParam.right_cam.fx; - right_cam_info_msg->K[2] = zedParam.right_cam.cx; - right_cam_info_msg->K[4] = zedParam.right_cam.fy; - right_cam_info_msg->K[5] = zedParam.right_cam.cy; - right_cam_info_msg->K[8] = 1.0; - - left_cam_info_msg->R.fill(0.0); - right_cam_info_msg->R.fill(0.0); - for (int i = 0; i < 3; i++) { + leftCamInfoMsg->D.resize(5); + rightCamInfoMsg->D.resize(5); + leftCamInfoMsg->D[0] = zedParam.left_cam.disto[0]; // k1 + leftCamInfoMsg->D[1] = zedParam.left_cam.disto[1]; // k2 + leftCamInfoMsg->D[2] = zedParam.left_cam.disto[4]; // k3 + leftCamInfoMsg->D[3] = zedParam.left_cam.disto[2]; // p1 + leftCamInfoMsg->D[4] = zedParam.left_cam.disto[3]; // p2 + rightCamInfoMsg->D[0] = zedParam.right_cam.disto[0]; // k1 + rightCamInfoMsg->D[1] = zedParam.right_cam.disto[1]; // k2 + rightCamInfoMsg->D[2] = zedParam.right_cam.disto[4]; // k3 + rightCamInfoMsg->D[3] = zedParam.right_cam.disto[2]; // p1 + rightCamInfoMsg->D[4] = zedParam.right_cam.disto[3]; // p2 + leftCamInfoMsg->K.fill(0.0); + rightCamInfoMsg->K.fill(0.0); + leftCamInfoMsg->K[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->K[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->K[4] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->K[5] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->K[8] = 1.0; + rightCamInfoMsg->K[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->K[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->K[4] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->K[5] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->K[8] = 1.0; + leftCamInfoMsg->R.fill(0.0); + rightCamInfoMsg->R.fill(0.0); + + for (size_t i = 0; i < 3; i++) { // identity - right_cam_info_msg->R[i + i * 3] = 1; - left_cam_info_msg->R[i + i * 3] = 1; + rightCamInfoMsg->R[i + i * 3] = 1; + leftCamInfoMsg->R[i + i * 3] = 1; } if (rawParam) { std::vector R_ = sl_tools::convertRodrigues(zedParam.R); float* p = R_.data(); for (int i = 0; i < 9; i++) { - right_cam_info_msg->R[i] = p[i]; + rightCamInfoMsg->R[i] = p[i]; } } - left_cam_info_msg->P.fill(0.0); - right_cam_info_msg->P.fill(0.0); - left_cam_info_msg->P[0] = zedParam.left_cam.fx; - left_cam_info_msg->P[2] = zedParam.left_cam.cx; - left_cam_info_msg->P[5] = zedParam.left_cam.fy; - left_cam_info_msg->P[6] = zedParam.left_cam.cy; - left_cam_info_msg->P[10] = 1.0; + leftCamInfoMsg->P.fill(0.0); + rightCamInfoMsg->P.fill(0.0); + leftCamInfoMsg->P[0] = static_cast(zedParam.left_cam.fx); + leftCamInfoMsg->P[2] = static_cast(zedParam.left_cam.cx); + leftCamInfoMsg->P[5] = static_cast(zedParam.left_cam.fy); + leftCamInfoMsg->P[6] = static_cast(zedParam.left_cam.cy); + leftCamInfoMsg->P[10] = 1.0; // http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html - right_cam_info_msg->P[3] = (-1 * zedParam.left_cam.fx * baseline); - - right_cam_info_msg->P[0] = zedParam.right_cam.fx; - right_cam_info_msg->P[2] = zedParam.right_cam.cx; - right_cam_info_msg->P[5] = zedParam.right_cam.fy; - right_cam_info_msg->P[6] = zedParam.right_cam.cy; - right_cam_info_msg->P[10] = 1.0; - - left_cam_info_msg->width = right_cam_info_msg->width = matWidth; - left_cam_info_msg->height = right_cam_info_msg->height = matHeight; - - left_cam_info_msg->header.frame_id = leftFrameId; - right_cam_info_msg->header.frame_id = rightFrameId; + rightCamInfoMsg->P[3] = static_cast(-1 * zedParam.left_cam.fx * baseline); + rightCamInfoMsg->P[0] = static_cast(zedParam.right_cam.fx); + rightCamInfoMsg->P[2] = static_cast(zedParam.right_cam.cx); + rightCamInfoMsg->P[5] = static_cast(zedParam.right_cam.fy); + rightCamInfoMsg->P[6] = static_cast(zedParam.right_cam.cy); + rightCamInfoMsg->P[10] = 1.0; + leftCamInfoMsg->width = rightCamInfoMsg->width = static_cast(mMatWidth); + leftCamInfoMsg->height = rightCamInfoMsg->height = static_cast(mMatHeight); + leftCamInfoMsg->header.frame_id = leftFrameId; + rightCamInfoMsg->header.frame_id = rightFrameId; } void ZEDWrapperNodelet::dynamicReconfCallback(zed_wrapper::ZedConfig& config, uint32_t level) { switch (level) { case 0: - confidence = config.confidence; - NODELET_INFO("Reconfigure confidence : %d", confidence); + mCamConfidence = config.confidence; + NODELET_INFO("Reconfigure confidence : %d", mCamConfidence); break; + case 1: - exposure = config.exposure; - NODELET_INFO("Reconfigure exposure : %d", exposure); + mCamExposure = config.exposure; + NODELET_INFO("Reconfigure exposure : %d", mCamExposure); break; + case 2: - gain = config.gain; - NODELET_INFO("Reconfigure gain : %d", gain); + mCamGain = config.gain; + NODELET_INFO("Reconfigure gain : %d", mCamGain); break; + case 3: - autoExposure = config.auto_exposure; - if (autoExposure) { - triggerAutoExposure = true; + mCamAutoExposure = config.auto_exposure; + + if (mCamAutoExposure) { + mTriggerAutoExposure = true; } + NODELET_INFO("Reconfigure auto control of exposure and gain : %s", - autoExposure ? "Enable" : "Disable"); + mCamAutoExposure ? "Enable" : "Disable"); break; - case 4: - matResizeFactor = config.mat_resize_factor; - NODELET_INFO("Reconfigure mat_resize_factor: %g", matResizeFactor); - dataMutex.lock(); - - matWidth = static_cast(camWidth * matResizeFactor); - matHeight = static_cast(camHeight * matResizeFactor); - NODELET_DEBUG_STREAM("Data Mat size : " << matWidth << "x" << matHeight); + case 4: + mCamMatResizeFactor = config.mat_resize_factor; + NODELET_INFO("Reconfigure mat_resize_factor: %g", mCamMatResizeFactor); + mCamDataMutex.lock(); + mMatWidth = static_cast(mCamWidth * mCamMatResizeFactor); + mMatHeight = static_cast(mCamHeight * mCamMatResizeFactor); + NODELET_DEBUG_STREAM("Data Mat size : " << mMatWidth << "x" << mMatHeight); // Update Camera Info - fillCamInfo(zed, leftCamInfoMsg, rightCamInfoMsg, leftCamOptFrameId, - rightCamOptFrameId); - fillCamInfo(zed, leftCamInfoRawMsg, rightCamInfoRawMsg, leftCamOptFrameId, - rightCamOptFrameId, true); - rgbCamInfoMsg = depthCamInfoMsg = leftCamInfoMsg; // the reference camera is + fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, + mRightCamOptFrameId); + fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, + mRightCamOptFrameId, true); + mRgbCamInfoMsg = mDepthCamInfoMsg = mLeftCamInfoMsg; // the reference camera is // the Left one (next to // the ZED logo) - rgbCamInfoRawMsg = leftCamInfoRawMsg; - - dataMutex.unlock(); + mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; + mCamDataMutex.unlock(); + break; + case 5: + mCamMaxDepth = config.max_depth; + NODELET_INFO("Reconfigure max depth : %g", mCamMaxDepth); break; } } + void ZEDWrapperNodelet::pathPubCallback(const ros::TimerEvent& e) { + uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); + uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); + + tf2::Transform base_to_odom; + base_to_odom.setIdentity(); + tf2::Transform base_to_map; + base_to_map.setIdentity(); + + // Look up the transformation from base frame to odom + try { + // Save the transformation from base to frame + geometry_msgs::TransformStamped b2o = + mTfBuffer->lookupTransform(mOdometryFrameId, mBaseFrameId, ros::Time(0)); + // Get the TF2 transformation + tf2::fromMsg(b2o.transform, base_to_odom); + } catch (tf2::TransformException& ex) { + NODELET_WARN_THROTTLE( + 10.0, "The tf from '%s' to '%s' does not seem to be available, " + "will assume it as identity!", + mBaseFrameId.c_str(), mOdometryFrameId.c_str()); + NODELET_DEBUG("Transform error: %s", ex.what()); + } + + if (mPublishMapTf) { + // Look up the transformation from base frame to map + try { + // Save the transformation from base to frame + geometry_msgs::TransformStamped b2m = + mTfBuffer->lookupTransform(mMapFrameId, mBaseFrameId, ros::Time(0)); + // Get the TF2 transformation + tf2::fromMsg(b2m.transform, base_to_map); + } catch (tf2::TransformException& ex) { + NODELET_WARN_THROTTLE( + 10.0, "The tf from '%s' to '%s' does not seem to be available, " + "will assume it as identity!", + mBaseFrameId.c_str(), mOdometryFrameId.c_str()); + NODELET_DEBUG("Transform error: %s", ex.what()); + } + } + + geometry_msgs::PoseStamped odomPose; + geometry_msgs::PoseStamped mapPose; + + odomPose.header.stamp = mFrameTimestamp; + odomPose.header.frame_id = mPublishMapTf ? mMapFrameId : mOdometryFrameId; // frame + // conversion from Tranform to message + geometry_msgs::Transform base2odom = tf2::toMsg(base_to_odom); + // Add all value in Pose message + odomPose.pose.position.x = base2odom.translation.x; + odomPose.pose.position.y = base2odom.translation.y; + odomPose.pose.position.z = base2odom.translation.z; + odomPose.pose.orientation.x = base2odom.rotation.x; + odomPose.pose.orientation.y = base2odom.rotation.y; + odomPose.pose.orientation.z = base2odom.rotation.z; + odomPose.pose.orientation.w = base2odom.rotation.w; + + if (mPublishMapTf) { + mapPose.header.stamp = mFrameTimestamp; + mapPose.header.frame_id = mMapFrameId; // map_frame + // conversion from Tranform to message + geometry_msgs::Transform base2map = tf2::toMsg(base_to_map); + // Add all value in Pose message + mapPose.pose.position.x = base2map.translation.x; + mapPose.pose.position.y = base2map.translation.y; + mapPose.pose.position.z = base2map.translation.z; + mapPose.pose.orientation.x = base2map.rotation.x; + mapPose.pose.orientation.y = base2map.rotation.y; + mapPose.pose.orientation.z = base2map.rotation.z; + mapPose.pose.orientation.w = base2map.rotation.w; + } + + // Circular vector + if (mPathMaxCount != -1) { + if (mOdomPath.size() == mPathMaxCount) { + NODELET_DEBUG("Path vectors full: rotating "); + std::rotate(mOdomPath.begin(), mOdomPath.begin() + 1, mOdomPath.end()); + std::rotate(mMapPath.begin(), mMapPath.begin() + 1, mMapPath.end()); + + mMapPath[mPathMaxCount - 1] = mapPose; + mOdomPath[mPathMaxCount - 1] = odomPose; + } else { + //NODELET_DEBUG_STREAM("Path vectors adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); + } + } else { + //NODELET_DEBUG_STREAM("No limit path vectors, adding last available poses"); + mMapPath.push_back(mapPose); + mOdomPath.push_back(odomPose); + } + + if (mapPathSub > 0 && mPublishMapTf) { + nav_msgs::Path mapPath; + mapPath.header.frame_id = mMapFrameId; + mapPath.header.stamp = mFrameTimestamp; + mapPath.poses = mMapPath; + + mPubMapPath.publish(mapPath); + } + + if (odomPathSub > 0) { + nav_msgs::Path odomPath; + odomPath.header.frame_id = mPublishMapTf ? mMapFrameId : mOdometryFrameId; + odomPath.header.stamp = mFrameTimestamp; + odomPath.poses = mOdomPath; + + mPubOdomPath.publish(odomPath); + } + + } + void ZEDWrapperNodelet::imuPubCallback(const ros::TimerEvent& e) { - int imu_SubNumber = pubImu.getNumSubscribers(); - int imu_RawSubNumber = pubImuRaw.getNumSubscribers(); + + std::lock_guard lock(mCloseZedMutex); + if (!mZed.isOpened()) { + return; + } + + uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); + uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); if (imu_SubNumber < 1 && imu_RawSubNumber < 1) { return; } - ros::Time t = - sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_IMAGE)); + ros::Time t; + if (mSvoMode) { + t = ros::Time::now(); + } else { + t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE_IMAGE)); + } sl::IMUData imu_data; - zed.getIMUData(imu_data, sl::TIME_REFERENCE_CURRENT); + mZed.getIMUData(imu_data, sl::TIME_REFERENCE_CURRENT); if (imu_SubNumber > 0) { sensor_msgs::Imu imu_msg; - imu_msg.header.stamp = imuTime; // t; - imu_msg.header.frame_id = imuFrameId; - - imu_msg.orientation.x = xSign * imu_data.getOrientation()[xIdx]; - imu_msg.orientation.y = ySign * imu_data.getOrientation()[yIdx]; - imu_msg.orientation.z = zSign * imu_data.getOrientation()[zIdx]; + imu_msg.header.stamp = mFrameTimestamp; // t; + imu_msg.header.frame_id = mImuFrameId; + imu_msg.orientation.x = mSignX * imu_data.getOrientation()[mIdxX]; + imu_msg.orientation.y = mSignY * imu_data.getOrientation()[mIdxY]; + imu_msg.orientation.z = mSignZ * imu_data.getOrientation()[mIdxZ]; imu_msg.orientation.w = imu_data.getOrientation()[3]; - - imu_msg.angular_velocity.x = xSign * imu_data.angular_velocity[xIdx]; - imu_msg.angular_velocity.y = ySign * imu_data.angular_velocity[yIdx]; - imu_msg.angular_velocity.z = zSign * imu_data.angular_velocity[zIdx]; - - imu_msg.linear_acceleration.x = xSign * imu_data.linear_acceleration[xIdx]; - imu_msg.linear_acceleration.y = ySign * imu_data.linear_acceleration[yIdx]; - imu_msg.linear_acceleration.z = zSign * imu_data.linear_acceleration[zIdx]; + imu_msg.angular_velocity.x = mSignX * imu_data.angular_velocity[mIdxX]; + imu_msg.angular_velocity.y = mSignY * imu_data.angular_velocity[mIdxY]; + imu_msg.angular_velocity.z = mSignZ * imu_data.angular_velocity[mIdxZ]; + imu_msg.linear_acceleration.x = mSignX * imu_data.linear_acceleration[mIdxX]; + imu_msg.linear_acceleration.y = mSignY * imu_data.linear_acceleration[mIdxY]; + imu_msg.linear_acceleration.z = mSignZ * imu_data.linear_acceleration[mIdxZ]; for (int i = 0; i < 3; i += 3) { imu_msg.orientation_covariance[i * 3 + 0] = - imu_data.orientation_covariance.r[i * 3 + xIdx]; + imu_data.orientation_covariance.r[i * 3 + mIdxX]; imu_msg.orientation_covariance[i * 3 + 1] = - imu_data.orientation_covariance.r[i * 3 + yIdx]; + imu_data.orientation_covariance.r[i * 3 + mIdxY]; imu_msg.orientation_covariance[i * 3 + 2] = - imu_data.orientation_covariance.r[i * 3 + zIdx]; - + imu_data.orientation_covariance.r[i * 3 + mIdxZ]; imu_msg.linear_acceleration_covariance[i * 3 + 0] = - imu_data.linear_acceleration_convariance.r[i * 3 + xIdx]; + imu_data.linear_acceleration_convariance.r[i * 3 + mIdxX]; imu_msg.linear_acceleration_covariance[i * 3 + 1] = - imu_data.linear_acceleration_convariance.r[i * 3 + yIdx]; + imu_data.linear_acceleration_convariance.r[i * 3 + mIdxY]; imu_msg.linear_acceleration_covariance[i * 3 + 2] = - imu_data.linear_acceleration_convariance.r[i * 3 + zIdx]; - + imu_data.linear_acceleration_convariance.r[i * 3 + mIdxZ]; imu_msg.angular_velocity_covariance[i * 3 + 0] = - imu_data.angular_velocity_convariance.r[i * 3 + xIdx]; + imu_data.angular_velocity_convariance.r[i * 3 + mIdxX]; imu_msg.angular_velocity_covariance[i * 3 + 1] = - imu_data.angular_velocity_convariance.r[i * 3 + yIdx]; + imu_data.angular_velocity_convariance.r[i * 3 + mIdxY]; imu_msg.angular_velocity_covariance[i * 3 + 2] = - imu_data.angular_velocity_convariance.r[i * 3 + zIdx]; + imu_data.angular_velocity_convariance.r[i * 3 + mIdxZ]; } - pubImu.publish(imu_msg); + mPubImu.publish(imu_msg); } if (imu_RawSubNumber > 0) { sensor_msgs::Imu imu_raw_msg; - imu_raw_msg.header.stamp = imuTime; // t; - imu_raw_msg.header.frame_id = imuFrameId; - - imu_raw_msg.angular_velocity.x = xSign * imu_data.angular_velocity[xIdx]; - imu_raw_msg.angular_velocity.y = ySign * imu_data.angular_velocity[yIdx]; - imu_raw_msg.angular_velocity.z = zSign * imu_data.angular_velocity[zIdx]; - + imu_raw_msg.header.stamp = mFrameTimestamp; // t; + imu_raw_msg.header.frame_id = mImuFrameId; + imu_raw_msg.angular_velocity.x = mSignX * imu_data.angular_velocity[mIdxX]; + imu_raw_msg.angular_velocity.y = mSignY * imu_data.angular_velocity[mIdxY]; + imu_raw_msg.angular_velocity.z = mSignZ * imu_data.angular_velocity[mIdxZ]; imu_raw_msg.linear_acceleration.x = - xSign * imu_data.linear_acceleration[xIdx]; + mSignX * imu_data.linear_acceleration[mIdxX]; imu_raw_msg.linear_acceleration.y = - ySign * imu_data.linear_acceleration[yIdx]; + mSignY * imu_data.linear_acceleration[mIdxY]; imu_raw_msg.linear_acceleration.z = - zSign * imu_data.linear_acceleration[zIdx]; + mSignZ * imu_data.linear_acceleration[mIdxZ]; for (int i = 0; i < 3; i += 3) { imu_raw_msg.linear_acceleration_covariance[i * 3 + 0] = - imu_data.linear_acceleration_convariance.r[i * 3 + xIdx]; + imu_data.linear_acceleration_convariance.r[i * 3 + mIdxX]; imu_raw_msg.linear_acceleration_covariance[i * 3 + 1] = - imu_data.linear_acceleration_convariance.r[i * 3 + yIdx]; + imu_data.linear_acceleration_convariance.r[i * 3 + mIdxY]; imu_raw_msg.linear_acceleration_covariance[i * 3 + 2] = - imu_data.linear_acceleration_convariance.r[i * 3 + zIdx]; - + imu_data.linear_acceleration_convariance.r[i * 3 + mIdxZ]; imu_raw_msg.angular_velocity_covariance[i * 3 + 0] = - imu_data.angular_velocity_convariance.r[i * 3 + xIdx]; + imu_data.angular_velocity_convariance.r[i * 3 + mIdxX]; imu_raw_msg.angular_velocity_covariance[i * 3 + 1] = - imu_data.angular_velocity_convariance.r[i * 3 + yIdx]; + imu_data.angular_velocity_convariance.r[i * 3 + mIdxY]; imu_raw_msg.angular_velocity_covariance[i * 3 + 2] = - imu_data.angular_velocity_convariance.r[i * 3 + zIdx]; + imu_data.angular_velocity_convariance.r[i * 3 + mIdxZ]; } imu_raw_msg.orientation_covariance[0] = -1; // Orientation data is not available in "data_raw" -> See ROS REP145 // http://www.ros.org/reps/rep-0145.html#topics - - pubImuRaw.publish(imu_raw_msg); + mPubImuRaw.publish(imu_raw_msg); } // Publish IMU tf only if enabled - if (publishTf) { - // Camera to map transform from TF buffer - tf2::Transform base_to_map; + if (mPublishTf) { + // Camera to pose transform from TF buffer + tf2::Transform cam_to_pose; + + std::string poseFrame; // Look up the transformation from base frame to map link try { + poseFrame = mPublishMapTf ? mMapFrameId : mOdometryFrameId; + // Save the transformation from base to frame - geometry_msgs::TransformStamped b2m = - tfBuffer->lookupTransform(mapFrameId, baseFrameId, ros::Time(0)); + geometry_msgs::TransformStamped c2p = + mTfBuffer->lookupTransform(poseFrame, mCameraFrameId, ros::Time(0)); // Get the TF2 transformation - tf2::fromMsg(b2m.transform, base_to_map); + tf2::fromMsg(c2p.transform, cam_to_pose); } catch (tf2::TransformException& ex) { NODELET_WARN_THROTTLE( 10.0, "The tf from '%s' to '%s' does not seem to be available. " "IMU TF not published!", - baseFrameId.c_str(), mapFrameId.c_str()); + mCameraFrameId.c_str(), mMapFrameId.c_str()); NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); return; } // IMU Quaternion in Map frame tf2::Quaternion imu_q; - imu_q.setX(xSign * imu_data.getOrientation()[xIdx]); - imu_q.setY(ySign * imu_data.getOrientation()[yIdx]); - imu_q.setZ(zSign * imu_data.getOrientation()[zIdx]); + imu_q.setX(mSignX * imu_data.getOrientation()[mIdxX]); + imu_q.setY(mSignY * imu_data.getOrientation()[mIdxY]); + imu_q.setZ(mSignZ * imu_data.getOrientation()[mIdxZ]); imu_q.setW(imu_data.getOrientation()[3]); - // Pose Quaternion from ZED Camera - tf2::Quaternion map_q = base_to_map.getRotation(); - + tf2::Quaternion map_q = cam_to_pose.getRotation(); // Difference between IMU and ZED Quaternion tf2::Quaternion delta_q = imu_q * map_q.inverse(); - tf2::Transform imu_pose; imu_pose.setIdentity(); imu_pose.setRotation(delta_q); - // Note, the frame is published, but its values will only change if someone // has subscribed to IMU - publishImuFrame(imu_pose); // publish the imu Frame + publishImuFrame(imu_pose, mFrameTimestamp); // publish the imu Frame } } - void ZEDWrapperNodelet::device_poll() { - ros::Rate loop_rate(frameRate); + void ZEDWrapperNodelet::device_poll_thread_func() { + ros::Rate loop_rate(mCamFrameRate); - ros::Time old_t = - sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT)); - imuTime = old_t; + // Timestamp initialization + if (mSvoMode) { + mFrameTimestamp = ros::Time::now(); + } else { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE_CURRENT)); + } - sl::ERROR_CODE grab_status; - trackingActivated = false; + mPrevFrameTimestamp = mFrameTimestamp; + sl::ERROR_CODE grab_status; + mTrackingActivated = false; // Get the parameters of the ZED images - camWidth = zed.getResolution().width; - camHeight = zed.getResolution().height; - NODELET_DEBUG_STREAM("Camera Frame size : " << camWidth << "x" << camHeight); - - matWidth = static_cast(camWidth * matResizeFactor); - matHeight = static_cast(camHeight * matResizeFactor); - NODELET_DEBUG_STREAM("Data Mat size : " << matWidth << "x" << matHeight); + mCamWidth = mZed.getResolution().width; + mCamHeight = mZed.getResolution().height; + NODELET_DEBUG_STREAM("Camera Frame size : " << mCamWidth << "x" << mCamHeight); + mMatWidth = static_cast(mCamWidth * mCamMatResizeFactor); + mMatHeight = static_cast(mCamHeight * mCamMatResizeFactor); + NODELET_DEBUG_STREAM("Data Mat size : " << mMatWidth << "x" << mMatHeight); // Create and fill the camera information messages - fillCamInfo(zed, leftCamInfoMsg, rightCamInfoMsg, leftCamOptFrameId, - rightCamOptFrameId); - fillCamInfo(zed, leftCamInfoRawMsg, rightCamInfoRawMsg, leftCamOptFrameId, - rightCamOptFrameId, true); - rgbCamInfoMsg = depthCamInfoMsg = leftCamInfoMsg; - rgbCamInfoRawMsg = leftCamInfoRawMsg; - + fillCamInfo(mZed, mLeftCamInfoMsg, mRightCamInfoMsg, mLeftCamOptFrameId, + mRightCamOptFrameId); + fillCamInfo(mZed, mLeftCamInfoRawMsg, mRightCamInfoRawMsg, mLeftCamOptFrameId, + mRightCamOptFrameId, true); + mRgbCamInfoMsg = mDepthCamInfoMsg = mLeftCamInfoMsg; // the reference camera is + // the Left one (next to the + // ZED logo) + mRgbCamInfoRawMsg = mLeftCamInfoRawMsg; sl::RuntimeParameters runParams; - runParams.sensing_mode = static_cast(sensingMode); + runParams.sensing_mode = static_cast(mCamSensingMode); + sl::Mat leftZEDMat, rightZEDMat, depthZEDMat, disparityZEDMat, confImgZEDMat, confMapZEDMat; - sl::Mat leftZEDMat, rightZEDMat, depthZEDMat, disparityZEDMat, confImgZEDMat, - confMapZEDMat; // Main loop - while (nhNs.ok()) { + while (mNhNs.ok()) { // Check for subscribers - int rgb_SubNumber = pubRgb.getNumSubscribers(); - int rgb_raw_SubNumber = pubRawRgb.getNumSubscribers(); - int left_SubNumber = pubLeft.getNumSubscribers(); - int left_raw_SubNumber = pubRawLeft.getNumSubscribers(); - int right_SubNumber = pubRight.getNumSubscribers(); - int right_raw_SubNumber = pubRawRight.getNumSubscribers(); - int depth_SubNumber = pubDepth.getNumSubscribers(); - int disparity_SubNumber = pubDisparity.getNumSubscribers(); - int cloud_SubNumber = pubCloud.getNumSubscribers(); - int pose_SubNumber = pubPose.getNumSubscribers(); - int odom_SubNumber = pubOdom.getNumSubscribers(); - int conf_img_SubNumber = pubConfImg.getNumSubscribers(); - int conf_map_SubNumber = pubConfMap.getNumSubscribers(); - int imu_SubNumber = pubImu.getNumSubscribers(); - int imu_RawSubNumber = pubImuRaw.getNumSubscribers(); - bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + - left_raw_SubNumber + right_SubNumber + right_raw_SubNumber + - depth_SubNumber + disparity_SubNumber + cloud_SubNumber + - pose_SubNumber + odom_SubNumber + conf_img_SubNumber + - conf_map_SubNumber + imu_SubNumber + imu_RawSubNumber) > 0; + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t cloudSubnumber = mPubCloud.getNumSubscribers(); + uint32_t poseSubnumber = mPubPose.getNumSubscribers(); + uint32_t poseCovSubnumber = mPubPoseCov.getNumSubscribers(); + uint32_t odomSubnumber = mPubOdom.getNumSubscribers(); + uint32_t confImgSubnumber = mPubConfImg.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t imuSubnumber = mPubImu.getNumSubscribers(); + uint32_t imuRawsubnumber = mPubImuRaw.getNumSubscribers(); + uint32_t pathSubNumber = mPubMapPath.getNumSubscribers() + mPubOdomPath.getNumSubscribers(); + bool runLoop = ((rgbSubnumber + rgbRawSubnumber + leftSubnumber + + leftRawSubnumber + rightSubnumber + rightRawSubnumber + + depthSubnumber + disparitySubnumber + cloudSubnumber + + poseSubnumber + poseCovSubnumber + odomSubnumber + confImgSubnumber + + confMapSubnumber + imuSubnumber + imuRawsubnumber + pathSubNumber) > 0); runParams.enable_point_cloud = false; - if (cloud_SubNumber > 0) { + + if (cloudSubnumber > 0) { runParams.enable_point_cloud = true; } + // Run the loop only if there is some subscribers if (runLoop) { - if ((depthStabilization || pose_SubNumber > 0 || odom_SubNumber > 0 || - cloud_SubNumber > 0 || depth_SubNumber > 0) && - !trackingActivated) { // Start the tracking + bool startTracking = (mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || + odomSubnumber > 0 || cloudSubnumber > 0 || depthSubnumber > 0 || pathSubNumber > 0); + + if ((startTracking) && !mTrackingActivated) { // Start the tracking start_tracking(); - } else if (!depthStabilization && pose_SubNumber == 0 && - odom_SubNumber == 0 && - trackingActivated) { // Stop the tracking - zed.disableTracking(); - trackingActivated = false; + } else if (!mDepthStabilization && poseSubnumber == 0 && poseCovSubnumber == 0 && + odomSubnumber == 0 && + mTrackingActivated) { // Stop the tracking + mZed.disableTracking(); + mTrackingActivated = false; } - computeDepth = (depth_SubNumber + disparity_SubNumber + cloud_SubNumber + - pose_SubNumber + odom_SubNumber + conf_img_SubNumber + - conf_map_SubNumber) > 0; // Detect if one of the - // subscriber need to have the - // depth information - // Timestamp - ros::Time t = - sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_IMAGE)); - - grabbing = true; - if (computeDepth) { - int actual_confidence = zed.getConfidenceThreshold(); - if (actual_confidence != confidence) { - zed.setConfidenceThreshold(confidence); + // Detect if one of the subscriber need to have the depth information + mComputeDepth = ((depthSubnumber + disparitySubnumber + cloudSubnumber + + poseSubnumber + poseCovSubnumber + odomSubnumber + confImgSubnumber + + confMapSubnumber) > 0); + + if (mComputeDepth) { + int actual_confidence = mZed.getConfidenceThreshold(); + + if (actual_confidence != mCamConfidence) { + mZed.setConfidenceThreshold(mCamConfidence); + } + + double actual_max_depth = static_cast(mZed.getDepthMaxRangeValue()); + + if (actual_max_depth != mCamMaxDepth) { + mZed.setDepthMaxRangeValue(static_cast(mCamMaxDepth)); } + runParams.enable_depth = true; // Ask to compute the depth } else { runParams.enable_depth = false; } - grab_status = zed.grab(runParams); // Ask to not compute the depth - - grabbing = false; + grab_status = mZed.grab(runParams); // Ask to not compute the depth // cout << toString(grab_status) << endl; - if (grab_status != - sl::ERROR_CODE::SUCCESS) { // Detect if a error occurred (for example: + if (grab_status != sl::ERROR_CODE::SUCCESS) { + // Detect if a error occurred (for example: // the zed have been disconnected) and // re-initialize the ZED - - if (grab_status == sl::ERROR_CODE_NOT_A_NEW_FRAME) { - NODELET_DEBUG_THROTTLE(1.0, "Wait for a new image to proceed"); - } else { + if (grab_status != sl::ERROR_CODE_NOT_A_NEW_FRAME) { NODELET_INFO_STREAM_ONCE(toString(grab_status)); } + // if ( mSvoMode && mPubClock.getNumSubscribers() > 0) { + // rosgraph_msgs::Clock clkMsg; + // clkMsg.clock = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE_IMAGE)); + + // mPubClock.publish(clkMsg); + // } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); - if ((t - old_t).toSec() > 5) { - zed.close(); + if ((ros::Time::now() - mPrevFrameTimestamp).toSec() > 5 && !mSvoMode) { + mCloseZedMutex.lock(); + mZed.close(); + mCloseZedMutex.unlock(); - NODELET_INFO("Re-opening the ZED"); sl::ERROR_CODE err = sl::ERROR_CODE_CAMERA_NOT_DETECTED; + while (err != sl::SUCCESS) { - if (!nhNs.ok()) { + if (!mNhNs.ok()) { mStopNode = true; - zed.close(); + + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); + NODELET_DEBUG("ZED pool thread finished"); return; } - int id = sl_tools::checkCameraReady(serial_number); - if (id > 0) { - param.camera_linux_id = id; - err = zed.open(param); // Try to initialize the ZED + int id = sl_tools::checkCameraReady(mZedSerialNumber); + + if (id >= 0) { + mZedParams.camera_linux_id = id; + err = mZed.open(mZedParams); // Try to initialize the ZED NODELET_INFO_STREAM(toString(err)); } else { - NODELET_INFO("Waiting for the ZED to be re-connected"); + NODELET_INFO_STREAM("Waiting for the ZED (S/N " << mZedSerialNumber << ") to be re-connected"); } + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); } - trackingActivated = false; - if (depthStabilization || pose_SubNumber > 0 || - odom_SubNumber > 0) { // Start the tracking + + mTrackingActivated = false; + + startTracking = mDepthStabilization || poseSubnumber > 0 || poseCovSubnumber > 0 || + odomSubnumber > 0; + + if (startTracking) { // Start the tracking start_tracking(); } } + continue; } - // Time update - old_t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT)); + mPrevFrameTimestamp = mFrameTimestamp; + + // Timestamp + if (mSvoMode) { + mFrameTimestamp = ros::Time::now(); + } else { + mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE_IMAGE)); + } - if (autoExposure) { + if (mCamAutoExposure) { // getCameraSettings() can't check status of auto exposure // triggerAutoExposure is used to execute setCameraSettings() only once - if (triggerAutoExposure) { - zed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, 0, true); - triggerAutoExposure = false; + if (mTriggerAutoExposure) { + mZed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, 0, true); + mTriggerAutoExposure = false; } } else { int actual_exposure = - zed.getCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE); - if (actual_exposure != exposure) { - zed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, exposure); + mZed.getCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE); + + if (actual_exposure != mCamExposure) { + mZed.setCameraSettings(sl::CAMERA_SETTINGS_EXPOSURE, mCamExposure); } - int actual_gain = zed.getCameraSettings(sl::CAMERA_SETTINGS_GAIN); - if (actual_gain != gain) { - zed.setCameraSettings(sl::CAMERA_SETTINGS_GAIN, gain); + int actual_gain = mZed.getCameraSettings(sl::CAMERA_SETTINGS_GAIN); + + if (actual_gain != mCamGain) { + mZed.setCameraSettings(sl::CAMERA_SETTINGS_GAIN, mCamGain); } } - dataMutex.lock(); + mCamDataMutex.lock(); // Publish the left == rgb image if someone has subscribed to - if (left_SubNumber > 0 || rgb_SubNumber > 0) { + if (leftSubnumber > 0 || rgbSubnumber > 0) { // Retrieve RGBA Left image - zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT, sl::MEM_CPU, matWidth, - matHeight); + mZed.retrieveImage(leftZEDMat, sl::VIEW_LEFT, sl::MEM_CPU, mMatWidth, mMatHeight); - if (left_SubNumber > 0) { - publishCamInfo(leftCamInfoMsg, pubLeftCamInfo, t); - publishImage(leftZEDMat, pubLeft, leftCamOptFrameId, t); + if (leftSubnumber > 0) { + publishCamInfo(mLeftCamInfoMsg, mPubLeftCamInfo, mFrameTimestamp); + publishImage(leftZEDMat, mPubLeft, mLeftCamOptFrameId, mFrameTimestamp); } - if (rgb_SubNumber > 0) { - publishCamInfo(rgbCamInfoMsg, pubRgbCamInfo, t); - publishImage(leftZEDMat, pubRgb, depthOptFrameId, - t); // rgb is the left image + + if (rgbSubnumber > 0) { + publishCamInfo(mRgbCamInfoMsg, mPubRgbCamInfo, mFrameTimestamp); + publishImage(leftZEDMat, mPubRgb, mDepthOptFrameId, mFrameTimestamp); // rgb is the left image } } // Publish the left_raw == rgb_raw image if someone has subscribed to - if (left_raw_SubNumber > 0 || rgb_raw_SubNumber > 0) { + if (leftRawSubnumber > 0 || rgbRawSubnumber > 0) { // Retrieve RGBA Left image - zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT_UNRECTIFIED, sl::MEM_CPU, - matWidth, matHeight); + mZed.retrieveImage(leftZEDMat, sl::VIEW_LEFT_UNRECTIFIED, sl::MEM_CPU, mMatWidth, mMatHeight); - if (left_raw_SubNumber > 0) { - publishCamInfo(leftCamInfoRawMsg, pubLeftCamInfoRaw, t); - publishImage(leftZEDMat, pubRawLeft, leftCamOptFrameId, t); + if (leftRawSubnumber > 0) { + publishCamInfo(mLeftCamInfoRawMsg, mPubLeftCamInfoRaw, mFrameTimestamp); + publishImage(leftZEDMat, mPubRawLeft, mLeftCamOptFrameId, mFrameTimestamp); } - if (rgb_raw_SubNumber > 0) { - publishCamInfo(rgbCamInfoRawMsg, pubRgbCamInfoRaw, t); - publishImage(leftZEDMat, pubRawRgb, depthOptFrameId, t); + + if (rgbRawSubnumber > 0) { + publishCamInfo(mRgbCamInfoRawMsg, mPubRgbCamInfoRaw, mFrameTimestamp); + publishImage(leftZEDMat, mPubRawRgb, mDepthOptFrameId, mFrameTimestamp); } } // Publish the right image if someone has subscribed to - if (right_SubNumber > 0) { + if (rightSubnumber > 0) { // Retrieve RGBA Right image - zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT, sl::MEM_CPU, matWidth, - matHeight); + mZed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT, sl::MEM_CPU, mMatWidth, mMatHeight); - publishCamInfo(rightCamInfoMsg, pubRightCamInfo, t); - publishImage(rightZEDMat, pubRight, rightCamOptFrameId, t); + publishCamInfo(mRightCamInfoMsg, mPubRightCamInfo, mFrameTimestamp); + publishImage(rightZEDMat, mPubRight, mRightCamOptFrameId, mFrameTimestamp); } // Publish the right image if someone has subscribed to - if (right_raw_SubNumber > 0) { + if (rightRawSubnumber > 0) { // Retrieve RGBA Right image - zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT_UNRECTIFIED, sl::MEM_CPU, - matWidth, matHeight); + mZed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT_UNRECTIFIED, sl::MEM_CPU, mMatWidth, mMatHeight); - publishCamInfo(rightCamInfoRawMsg, pubRightCamInfoRaw, t); - publishImage(rightZEDMat, pubRawRight, rightCamOptFrameId, t); + publishCamInfo(mRightCamInfoRawMsg, mPubRightCamInfoRaw, mFrameTimestamp); + publishImage(rightZEDMat, mPubRawRight, mRightCamOptFrameId, mFrameTimestamp); } // Publish the depth image if someone has subscribed to - if (depth_SubNumber > 0 || disparity_SubNumber > 0) { - zed.retrieveMeasure(depthZEDMat, sl::MEASURE_DEPTH, sl::MEM_CPU, - matWidth, matHeight); - publishCamInfo(depthCamInfoMsg, pubDepthCamInfo, t); - publishDepth(depthZEDMat, t); + if (depthSubnumber > 0 || disparitySubnumber > 0) { + mZed.retrieveMeasure(depthZEDMat, sl::MEASURE_DEPTH, sl::MEM_CPU, mMatWidth, mMatHeight); + publishCamInfo(mDepthCamInfoMsg, mPubDepthCamInfo, mFrameTimestamp); + publishDepth(depthZEDMat, mFrameTimestamp); // in meters } // Publish the disparity image if someone has subscribed to - if (disparity_SubNumber > 0) { - zed.retrieveMeasure(disparityZEDMat, sl::MEASURE_DISPARITY, sl::MEM_CPU, - matWidth, matHeight); - + if (disparitySubnumber > 0) { + mZed.retrieveMeasure(disparityZEDMat, sl::MEASURE_DISPARITY, sl::MEM_CPU, mMatWidth, mMatHeight); // Need to flip sign, but cause of this is not sure - int dataSize = disparityZEDMat.getWidth() * disparityZEDMat.getHeight(); - sl::float1* dispDataPtr = disparityZEDMat.getPtr(); - - for (int i = 0; i < dataSize; i++) { - *(dispDataPtr++) *= -1.0f; - } - - publishDisparity(disparityZEDMat, t); + publishDisparity(disparityZEDMat, mFrameTimestamp); } // Publish the confidence image if someone has subscribed to - if (conf_img_SubNumber > 0) { - zed.retrieveImage(confImgZEDMat, sl::VIEW_CONFIDENCE, sl::MEM_CPU, - matWidth, matHeight); - - publishImage(confImgZEDMat, pubConfImg, confidenceOptFrameId, t); + if (confImgSubnumber > 0) { + mZed.retrieveImage(confImgZEDMat, sl::VIEW_CONFIDENCE, sl::MEM_CPU, mMatWidth, mMatHeight); + publishImage(confImgZEDMat, mPubConfImg, mConfidenceOptFrameId, mFrameTimestamp); } // Publish the confidence map if someone has subscribed to - if (conf_map_SubNumber > 0) { - zed.retrieveMeasure(confMapZEDMat, sl::MEASURE_CONFIDENCE, sl::MEM_CPU, - matWidth, matHeight); + if (confMapSubnumber > 0) { + mZed.retrieveMeasure(confMapZEDMat, sl::MEASURE_CONFIDENCE, sl::MEM_CPU, mMatWidth, mMatHeight); - pubConfMap.publish(imageToROSmsg(confMapZEDMat, confidenceOptFrameId, t)); + + mPubConfMap.publish(sl_tools::imageToROSmsg(confMapZEDMat, mConfidenceOptFrameId, mFrameTimestamp)); } // Publish the point cloud if someone has subscribed to - if (cloud_SubNumber > 0) { + if (cloudSubnumber > 0) { // Run the point cloud conversion asynchronously to avoid slowing down // all the program // Retrieve raw pointCloud data if latest Pointcloud is ready std::unique_lock lock(mPcMutex, std::defer_lock); if (lock.try_lock()) { - zed.retrieveMeasure(cloud, sl::MEASURE_XYZBGRA, sl::MEM_CPU, matWidth, matHeight); + mZed.retrieveMeasure(mCloud, sl::MEASURE_XYZBGRA, sl::MEM_CPU, mMatWidth, mMatHeight); - pointCloudFrameId = depthFrameId; - pointCloudTime = t; + mPointCloudFrameId = mDepthFrameId; + mPointCloudTime = mFrameTimestamp; // Signal Pointcloud thread that a new pointcloud is ready mPcDataReady = true; @@ -1554,130 +1764,135 @@ namespace zed_wrapper { } } - dataMutex.unlock(); - - // Transform from base to sensor - tf2::Transform sensor_to_base_transf; - // Look up the transformation from base frame to camera link - try { - // Save the transformation from base to frame - geometry_msgs::TransformStamped s2b = - tfBuffer->lookupTransform(baseFrameId, depthFrameId, t); - // Get the TF2 transformation - tf2::fromMsg(s2b.transform, sensor_to_base_transf); - - } catch (tf2::TransformException& ex) { - NODELET_WARN_THROTTLE( - 10.0, "The tf from '%s' to '%s' does not seem to be available, " - "will assume it as identity!", - depthFrameId.c_str(), baseFrameId.c_str()); - NODELET_DEBUG("Transform error: %s", ex.what()); - sensor_to_base_transf.setIdentity(); - } + mCamDataMutex.unlock(); + + // Publish the odometry if someone has subscribed to - if (pose_SubNumber > 0 || odom_SubNumber > 0 || cloud_SubNumber > 0 || depth_SubNumber > 0 || - imu_SubNumber > 0 || imu_RawSubNumber > 0) { - if (!initOdomWithPose) { + if (poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || cloudSubnumber > 0 || + depthSubnumber > 0 || imuSubnumber > 0 || imuRawsubnumber > 0 || pathSubNumber > 0) { + if (!mInitOdomWithPose) { sl::Pose deltaOdom; - zed.getPosition(deltaOdom, sl::REFERENCE_FRAME_CAMERA); - - // Transform ZED delta odom pose in TF2 Transformation - geometry_msgs::Transform deltaTransf; - - sl::Translation translation = deltaOdom.getTranslation(); - sl::Orientation quat = deltaOdom.getOrientation(); - - deltaTransf.translation.x = xSign * translation(xIdx); - deltaTransf.translation.y = ySign * translation(yIdx); - deltaTransf.translation.z = zSign * translation(zIdx); - - deltaTransf.rotation.x = xSign * quat(xIdx); - deltaTransf.rotation.y = ySign * quat(yIdx); - deltaTransf.rotation.z = zSign * quat(zIdx); - deltaTransf.rotation.w = quat(3); - - tf2::Transform deltaOdomTf; - tf2::fromMsg(deltaTransf, deltaOdomTf); - - // delta odom from sensor to base frame - tf2::Transform deltaOdomTf_base = sensor_to_base_transf * - deltaOdomTf * - sensor_to_base_transf.inverse(); - - // Propagate Odom transform in time - baseToOdomTransform = baseToOdomTransform * deltaOdomTf_base; - - // Publish odometry message - publishOdom(baseToOdomTransform, t); + sl::TRACKING_STATE status = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME_CAMERA); + + if (status == sl::TRACKING_STATE_OK || status == sl::TRACKING_STATE_SEARCHING || + status == sl::TRACKING_STATE_FPS_TOO_LOW) { + // Transform ZED delta odom pose in TF2 Transformation + geometry_msgs::Transform deltaTransf; + sl::Translation translation = deltaOdom.getTranslation(); + sl::Orientation quat = deltaOdom.getOrientation(); + deltaTransf.translation.x = mSignX * translation(mIdxX); + deltaTransf.translation.y = mSignY * translation(mIdxY); + deltaTransf.translation.z = mSignZ * translation(mIdxZ); + deltaTransf.rotation.x = mSignX * quat(mIdxX); + deltaTransf.rotation.y = mSignY * quat(mIdxY); + deltaTransf.rotation.z = mSignZ * quat(mIdxZ); + deltaTransf.rotation.w = quat(3); + tf2::Transform deltaOdomTf; + tf2::fromMsg(deltaTransf, deltaOdomTf); + // delta odom from sensor to base frame + tf2::Transform deltaOdomTf_base = + mSensor2BaseTransf * deltaOdomTf * mSensor2BaseTransf.inverse(); + + // Propagate Odom transform in time + mBase2OdomTransf = mBase2OdomTransf * deltaOdomTf_base; + // Publish odometry message + publishOdom(mBase2OdomTransf, deltaOdom, mFrameTimestamp); + mTrackingReady = true; + } else { + NODELET_DEBUG_STREAM("ODOM -> Tracking Status: " << sl::toString(status)); + } } } // Publish the zed camera pose if someone has subscribed to - if (pose_SubNumber > 0 || odom_SubNumber > 0 || cloud_SubNumber > 0 || depth_SubNumber > 0 || - imu_SubNumber > 0 || imu_RawSubNumber > 0) { - sl::Pose zed_pose; // Sensor to Map transform - zed.getPosition(zed_pose, sl::REFERENCE_FRAME_WORLD); - - // Transform ZED pose in TF2 Transformation - geometry_msgs::Transform sens2mapTransf; - - sl::Translation translation = zed_pose.getTranslation(); - sl::Orientation quat = zed_pose.getOrientation(); - - sens2mapTransf.translation.x = xSign * translation(xIdx); - sens2mapTransf.translation.y = ySign * translation(yIdx); - sens2mapTransf.translation.z = zSign * translation(zIdx); + if (poseSubnumber > 0 || odomSubnumber > 0 || poseCovSubnumber > 0 || cloudSubnumber > 0 || + depthSubnumber > 0 || imuSubnumber > 0 || imuRawsubnumber > 0 || pathSubNumber > 0) { + + static sl::TRACKING_STATE oldStatus; + sl::TRACKING_STATE status = mZed.getPosition(mLastZedPose, sl::REFERENCE_FRAME_WORLD); + + sl::Translation translation = mLastZedPose.getTranslation(); + sl::Orientation quat = mLastZedPose.getOrientation(); + + NODELET_DEBUG("POSE [%s] - %.2f,%.2f,%.2f %.2f,%.2f,%.2f,%.2f", + sl::toString(status).c_str(), + translation(mIdxX), translation(mIdxY), translation(mIdxZ), + quat(mIdxX), quat(mIdxY), quat(mIdxZ), quat(3)); + + if (status == sl::TRACKING_STATE_OK || + status == sl::TRACKING_STATE_SEARCHING /*|| status == sl::TRACKING_STATE_FPS_TOO_LOW*/) { + // Transform ZED pose in TF2 Transformation + geometry_msgs::Transform sens2mapTransf; + + sens2mapTransf.translation.x = mSignX * translation(mIdxX); + sens2mapTransf.translation.y = mSignY * translation(mIdxY); + sens2mapTransf.translation.z = mSignZ * translation(mIdxZ); + sens2mapTransf.rotation.x = mSignX * quat(mIdxX); + sens2mapTransf.rotation.y = mSignY * quat(mIdxY); + sens2mapTransf.rotation.z = mSignZ * quat(mIdxZ); + sens2mapTransf.rotation.w = quat(3); + tf2::Transform sens_to_map_transf; + tf2::fromMsg(sens2mapTransf, sens_to_map_transf); + // Transformation from camera sensor to base frame + /*tf2::Transform base_to_map_transform = + sensor_to_base_transf * sens_to_map_transf * sensor_to_base_transf.inverse();*/ + + tf2::Transform base_to_map_transform = (mSensor2BaseTransf * sens_to_map_transf.inverse()).inverse(); + + bool initOdom = false; + + if (!(mFloorAlignment)) { + initOdom = mInitOdomWithPose; + } else { + initOdom = (status == sl::TRACKING_STATE_OK) & mInitOdomWithPose; + } - sens2mapTransf.rotation.x = xSign * quat(xIdx); - sens2mapTransf.rotation.y = ySign * quat(yIdx); - sens2mapTransf.rotation.z = zSign * quat(zIdx); - sens2mapTransf.rotation.w = quat(3); + if (initOdom || mResetOdom) { - tf2::Transform sens_to_map_transf; - tf2::fromMsg(sens2mapTransf, sens_to_map_transf); + ROS_INFO("Odometry aligned to last tracking pose"); - // Transformation from camera sensor to base frame - tf2::Transform base_to_map_transform = sensor_to_base_transf * - sens_to_map_transf * - sensor_to_base_transf.inverse(); + // Propagate Odom transform in time + mBase2OdomTransf = base_to_map_transform; + base_to_map_transform.setIdentity(); - if (initOdomWithPose) { - // Propagate Odom transform in time - baseToOdomTransform = base_to_map_transform; - base_to_map_transform.setIdentity(); + if (odomSubnumber > 0) { + // Publish odometry message + publishOdom(mBase2OdomTransf, mLastZedPose, mFrameTimestamp); + } - if (odom_SubNumber > 0) { - // Publish odometry message - publishOdom(baseToOdomTransform, t); + mInitOdomWithPose = false; + mResetOdom = false; + } else { + // Transformation from map to odometry frame + mOdom2MapTransf = + base_to_map_transform * mBase2OdomTransf.inverse(); } - initOdomWithPose = false; + // Publish Pose message + publishPose(mFrameTimestamp); + mTrackingReady = true; } else { - // Transformation from map to odometry frame - odomToMapTransform = - base_to_map_transform * baseToOdomTransform.inverse(); + NODELET_DEBUG_STREAM("MAP -> Tracking Status: " << static_cast(status)); } - // Publish Pose message - publishPose(odomToMapTransform, t); + oldStatus = status; } // Publish pose tf only if enabled - if (publishTf) { + if (mPublishTf) { // Note, the frame is published, but its values will only change if // someone has subscribed to odom - publishOdomFrame(baseToOdomTransform, - t); // publish the base Frame in odometry frame - // Note, the frame is published, but its values will only change if - // someone has subscribed to map - publishPoseFrame(odomToMapTransform, - t); // publish the odometry Frame in map frame - - imuTime = t; + publishOdomFrame(mBase2OdomTransf, mFrameTimestamp); // publish the base Frame in odometry frame + if (mPublishMapTf) { + // Note, the frame is published, but its values will only change if + // someone has subscribed to map + publishPoseFrame(mOdom2MapTransf, mFrameTimestamp); // publish the odometry Frame in map frame + } } static int rateWarnCount = 0; + if (!loop_rate.sleep()) { rateWarnCount++; @@ -1697,28 +1912,37 @@ namespace zed_wrapper { rateWarnCount = 0; } } else { - NODELET_DEBUG_THROTTLE(1.0, "No topics subscribed by users"); + NODELET_DEBUG_THROTTLE(5.0, "No topics subscribed by users"); // Publish odometry tf only if enabled - if (publishTf) { - ros::Time t = - sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT)); - publishOdomFrame(baseToOdomTransform, - t); // publish the base Frame in odometry frame - publishPoseFrame(odomToMapTransform, - t); // publish the odometry Frame in map frame + if (mPublishTf) { + ros::Time t; + + if (mSvoMode) { + t = ros::Time::now(); + } else { + t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE_CURRENT)); + } + + publishOdomFrame(mBase2OdomTransf, mFrameTimestamp); // publish the base Frame in odometry frame + + if (mPublishMapTf) { + publishPoseFrame(mOdom2MapTransf, mFrameTimestamp); // publish the odometry Frame in map frame + } } + std::this_thread::sleep_for( std::chrono::milliseconds(10)); // No subscribers, we just wait - loop_rate.reset(); } } // while loop mStopNode = true; // Stops other threads - zed.close(); + + std::lock_guard lock(mCloseZedMutex); + NODELET_DEBUG("Closing ZED"); + mZed.close(); NODELET_DEBUG("ZED pool thread finished"); } - } // namespace diff --git a/zed_wrapper/src/tools/include/sl_tools.h b/zed_wrapper/src/tools/include/sl_tools.h index 568743d8..3d0db999 100644 --- a/zed_wrapper/src/tools/include/sl_tools.h +++ b/zed_wrapper/src/tools/include/sl_tools.h @@ -21,9 +21,10 @@ // /////////////////////////////////////////////////////////////////////////// -#include -#include #include +#include +#include +#include namespace sl_tools { @@ -56,18 +57,14 @@ namespace sl_tools { */ ros::Time slTime2Ros(sl::timeStamp t); - inline sl::uchar4 depackColor4(float colorIn) { - sl::uchar4 out; - uint32_t color_uint = *(uint32_t*) & colorIn; - unsigned char* color_uchar = (unsigned char*) &color_uint; - for (int c = 0; c < 3; c++) { - out[c] = static_cast(color_uchar[c]); - } - out.w = 255; - return out; - } + /* \brief sl::Mat to ros message conversion + * \param img : the image to publish + * \param frameId : the id of the reference frame of the image + * \param t : the ros::Time to stamp the image + */ + sensor_msgs::ImagePtr imageToROSmsg(sl::Mat img, std::string frameId, ros::Time t); + - } // namespace #endif // SL_TOOLS_H diff --git a/zed_wrapper/src/tools/src/sl_tools.cpp b/zed_wrapper/src/tools/src/sl_tools.cpp index 694fbd3c..cd4cd713 100644 --- a/zed_wrapper/src/tools/src/sl_tools.cpp +++ b/zed_wrapper/src/tools/src/sl_tools.cpp @@ -20,30 +20,40 @@ #include "sl_tools.h" +#include #include #include -#include + +#include + +#include namespace sl_tools { int checkCameraReady(unsigned int serial_number) { int id = -1; auto f = sl::Camera::getDeviceList(); + for (auto& it : f) - if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) { + if (it.serial_number == serial_number && + it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) { id = it.id; } + return id; } sl::DeviceProperties getZEDFromSN(unsigned int serial_number) { sl::DeviceProperties prop; auto f = sl::Camera::getDeviceList(); + for (auto& it : f) { - if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) { + if (it.serial_number == serial_number && + it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) { prop = it; } } + return prop; } @@ -121,6 +131,7 @@ namespace sl_tools { return R; } + bool file_exist(const std::string& name) { struct stat buffer; return (stat(name.c_str(), &buffer) == 0); @@ -128,11 +139,10 @@ namespace sl_tools { std::string getSDKVersion(int& major, int& minor, int& sub_minor) { std::string ver = sl::Camera::getSDKVersion().c_str(); - std::vector strings; std::istringstream f(ver); std::string s; - + while (getline(f, s, '.')) { strings.push_back(s); } @@ -156,9 +166,67 @@ namespace sl_tools { } ros::Time slTime2Ros(sl::timeStamp t) { - uint32_t sec = static_cast(t / 1000000000); + uint32_t sec = static_cast(t / 1000000000); uint32_t nsec = static_cast(t % 1000000000); return ros::Time(sec, nsec); } + sensor_msgs::ImagePtr imageToROSmsg(sl::Mat img, std::string frameId, ros::Time t) { + + sensor_msgs::ImagePtr ptr = boost::make_shared(); + sensor_msgs::Image& imgMessage = *ptr; + + imgMessage.header.stamp = t; + imgMessage.header.frame_id = frameId; + imgMessage.height = img.getHeight(); + imgMessage.width = img.getWidth(); + + int num = 1; // for endianness detection + imgMessage.is_bigendian = !(*(char*)&num == 1); + + imgMessage.step = img.getStepBytes(); + + size_t size = imgMessage.step * imgMessage.height; + imgMessage.data.resize(size); + + sl::MAT_TYPE dataType = img.getDataType(); + + switch (dataType) { + case sl::MAT_TYPE_32F_C1: /**< float 1 channel.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_32F_C2: /**< float 2 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC2; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_32F_C3: /**< float 3 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC3; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_32F_C4: /**< float 4 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC4; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C1: /**< unsigned char 1 channel.*/ + imgMessage.encoding = sensor_msgs::image_encodings::MONO8; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C2: /**< unsigned char 2 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_8UC2; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C3: /**< unsigned char 3 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::BGR8; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C4: /**< unsigned char 4 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::BGRA8; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + } + + return ptr; + } + } // namespace