Skip to content

Fixed issue when TF publishing is disabled #310

New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Merged
adujardin merged 1 commit into from
Oct 15, 2018
Merged

Fixed issue when TF publishing is disabled #310

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion zed_wrapper/launch/zed_camera.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
<param name="publish_tf" value="$(arg publish_tf)" />
<param name="publish_map_tf" value="$(arg publish_map_tf)" />


<!-- flip camera -->
<param name="camera_flip" value="$(arg camera_flip)" />

Expand Down
54 changes: 54 additions & 0 deletions zed_wrapper/launch/zed_no_tf.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<!--
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.
-->
<launch>
<arg name="svo_file" default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->

<arg name="camera_model" default="1" /> <!-- 0=ZED, 1=ZEDM-->
<arg name="serial_number" default="0" />
<arg name="verbose" default="true" />
<arg name="resolution" default="2" /> <!--0=RESOLUTION_HD2K, 1=RESOLUTION_HD1080, 2=RESOLUTION_HD720, 3=RESOLUTION_VGA -->
<arg name="frame_rate" default="30" />
<!-- RESOLUTION_HD2K -> 2208*1242, available framerates: 15 fps.
RESOLUTION_HD1080 -> 1920*1080, available framerates: 15, 30 fps.
RESOLUTION_HD720 -> 1280*720, available framerates: 15, 30, 60 fps.
RESOLUTION_VGA -> 672*376, available framerates: 15, 30, 60, 100 fps. -->

<arg name="publish_tf" default="false" />
<arg name="publish_map_tf" default="false" />
<arg name="publish_urdf" default="true" />

<arg name="pose_frame" default="odom" />
<arg name="odometry_frame" default="odom" />

<group ns="zed">
<include file="$(find zed_wrapper)/launch/zed_camera.launch">
<arg name="svo_file" value="$(arg svo_file)" />
<arg name="camera_model" value="$(arg camera_model)" />
<arg name="serial_number" value="$(arg serial_number)" />
<arg name="resolution" value="$(arg resolution)" />
<arg name="frame_rate" value="$(arg frame_rate)" />
<arg name="verbose" value="$(arg verbose)" />
<arg name="publish_tf" value="$(arg publish_tf)" />
<arg name="publish_map_tf" value="$(arg publish_map_tf)" />
<arg name="publish_urdf" value="$(arg publish_urdf)" />
<arg name="pose_frame" value="$(arg pose_frame)" />
<arg name="odometry_frame" value="$(arg odometry_frame)" />
</include>
</group>
</launch>
90 changes: 57 additions & 33 deletions zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,7 @@ namespace zed_wrapper {
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 (mOpenniDepthMode) {
NODELET_INFO_STREAM("Openni depth mode activated");
Expand Down Expand Up @@ -489,14 +489,14 @@ namespace zed_wrapper {
}
mPubOdom = mNh.advertise<nav_msgs::Odometry>(odometry_topic, 1);
NODELET_INFO_STREAM("Advertised on topic " << odometry_topic);

// Camera Path
if (mPathPubRate > 0) {
mPubOdomPath = mNh.advertise<nav_msgs::Path>(odom_path_topic, 1, true);
NODELET_INFO_STREAM("Advertised on topic " << odom_path_topic);
mPubMapPath = mNh.advertise<nav_msgs::Path>(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);

Expand All @@ -507,6 +507,8 @@ namespace zed_wrapper {

NODELET_DEBUG_STREAM("Path vector sizes: " << mOdomPath.size() << " " << mMapPath.size());
}
} else {
NODELET_INFO_STREAM("Path topics not published -> mPathPubRate: " << mPathPubRate);
}

// Imu publisher
Expand All @@ -526,7 +528,7 @@ namespace zed_wrapper {
<< mImuPubRate << " Hz"
<< " but ZED camera model does not support IMU data publishing.");
}

// Services
mSrvSetInitPose = mNh.advertiseService("set_initial_pose", &ZEDWrapperNodelet::on_set_pose, this);
mSrvResetOdometry = mNh.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this);
Expand Down Expand Up @@ -852,7 +854,7 @@ namespace zed_wrapper {
mBaseFrameId.c_str(), mMapFrameId.c_str());
NODELET_DEBUG("Transform error: %s", ex.what());
}
} else {
} else if (mPublishTf) {
// Look up the transformation from base frame to odom frame
try {
// Save the transformation from base to frame
Expand All @@ -873,18 +875,32 @@ namespace zed_wrapper {
header.stamp = mFrameTimestamp;
header.frame_id = mPublishMapTf ? mMapFrameId : mOdometryFrameId; // frame

// conversion from Tranform to message
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 (mPublishTf) {
// conversion from Tranform to message
geometry_msgs::Transform base2frame = tf2::toMsg(base_pose);

// Add all value in Pose message
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;
} else {
sl::Translation translation = mLastZedPose.getTranslation();
sl::Orientation quat = mLastZedPose.getOrientation();

pose.position.x = mSignX * translation(mIdxX);
pose.position.y = mSignY * translation(mIdxY);
pose.position.z = mSignZ * translation(mIdxZ);
pose.orientation.x = mSignX * quat(mIdxX);
pose.orientation.y = mSignY * quat(mIdxY);
pose.orientation.z = mSignZ * quat(mIdxZ);
pose.orientation.w = quat(3);
}

if (mPubPose.getNumSubscribers() > 0) {

Expand Down Expand Up @@ -1234,18 +1250,20 @@ namespace zed_wrapper {
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 (mPublishTf) {
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) {
Expand Down Expand Up @@ -1388,7 +1406,7 @@ namespace zed_wrapper {
} else {
r = mIdxZ;
}

imu_msg.orientation_covariance[i * 3 + 0] =
imu_data.orientation_covariance.r[r * 3 + mIdxX] * DEG2RAD * DEG2RAD;
imu_msg.orientation_covariance[i * 3 + 1] =
Expand Down Expand Up @@ -1438,7 +1456,7 @@ namespace zed_wrapper {
} else {
r = mIdxZ;
}

imu_raw_msg.linear_acceleration_covariance[i * 3 + 0] =
imu_data.linear_acceleration_convariance.r[r * 3 + mIdxX];
imu_raw_msg.linear_acceleration_covariance[i * 3 + 1] =
Expand Down Expand Up @@ -1803,20 +1821,26 @@ namespace zed_wrapper {
mCamDataMutex.unlock();



// Publish the odometry if someone has subscribed to
if (poseSubnumber > 0 || poseCovSubnumber > 0 || odomSubnumber > 0 || cloudSubnumber > 0 ||
depthSubnumber > 0 || imuSubnumber > 0 || imuRawsubnumber > 0 || pathSubNumber > 0) {
if (!mInitOdomWithPose) {
sl::Pose deltaOdom;
sl::TRACKING_STATE status = mZed.getPosition(deltaOdom, sl::REFERENCE_FRAME_CAMERA);

sl::Translation translation = deltaOdom.getTranslation();
sl::Orientation quat = deltaOdom.getOrientation();

NODELET_DEBUG("delta ODOM [%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 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);
Expand Down