diff --git a/launch/zed_camera.launch b/launch/zed_camera.launch index 7df6ac5d..a34bdd90 100644 --- a/launch/zed_camera.launch +++ b/launch/zed_camera.launch @@ -50,6 +50,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + @@ -61,6 +63,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + diff --git a/src/nodelet/include/zed_wrapper_nodelet.hpp b/src/nodelet/include/zed_wrapper_nodelet.hpp index fc21819f..69769453 100644 --- a/src/nodelet/include/zed_wrapper_nodelet.hpp +++ b/src/nodelet/include/zed_wrapper_nodelet.hpp @@ -284,6 +284,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { boost::shared_ptr tfBuffer; boost::shared_ptr tfListener; bool publishTf; + bool cameraFlip; // Launch file parameters int resolution; diff --git a/src/nodelet/src/zed_wrapper_nodelet.cpp b/src/nodelet/src/zed_wrapper_nodelet.cpp index 71612d03..8c02b21f 100644 --- a/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -132,6 +132,8 @@ void ZEDWrapperNodelet::onInit() { // Publish odometry tf nhNs.param("publish_tf", publishTf, true); + nhNs.param("camera_flip", cameraFlip, false); + if (serial_number > 0) NODELET_INFO_STREAM("SN : " << serial_number); @@ -152,6 +154,7 @@ void ZEDWrapperNodelet::onInit() { // Status of map TF NODELET_INFO_STREAM("Publish " << mapFrameId << " [" << (publishTf ? "TRUE" : "FALSE") << "]"); + NODELET_INFO_STREAM("Camera Flip [" << (cameraFlip ? "TRUE" : "FALSE") << "]"); // Status of odometry TF // NODELET_INFO_STREAM("Publish " << odometry_frame_id << " [" << (publish_tf @@ -338,7 +341,8 @@ void ZEDWrapperNodelet::onInit() { param.sdk_verbose = verbose; param.sdk_gpu_id = gpuId; param.depth_stabilization = depthStabilization; - + param.camera_image_flip = cameraFlip; + sl::ERROR_CODE err = sl::ERROR_CODE_CAMERA_NOT_DETECTED; while (err != sl::SUCCESS) { err = zed.open(param);