Skip to content

Commit

Permalink
Fixed issue when TF publishing is disabled (#310)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Myzhar @adujardin
Myzhar authored and adujardin committed Oct 15, 2018
1 parent 6d0aa61 commit 14a1a8f
Show file tree
Hide file tree
Showing 3 changed files with 111 additions and 34 deletions.
1 change: 0 additions & 1 deletion zed_wrapper/launch/zed_camera.launch
View file
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
@@ -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
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

0 comments on commit 14a1a8f

Please sign in to comment.