tf_visual_tools.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman <dave@dav.ee>
00036    Desc:   Helps debug and visualize transforms via the TF infrastructure
00037 */
00038 
00039 #include <rviz_visual_tools/tf_visual_tools.h>
00040 
00041 // TF
00042 #include <eigen_conversions/eigen_msg.h>
00043 
00044 // C++
00045 #include <string>
00046 
00047 namespace rviz_visual_tools
00048 {
00049 TFVisualTools::TFVisualTools()
00050 {
00051   double loop_hz = 2;  // hz
00052   ros::Duration update_freq = ros::Duration(1.0 / loop_hz);
00053   non_realtime_loop_ = nh_.createTimer(update_freq, &TFVisualTools::publishAllTransforms, this);
00054 
00055   ROS_INFO_STREAM_NAMED("tf_visual_tools", "TFVisualTools Ready.");
00056 }
00057 
00058 bool TFVisualTools::publishTransform(const Eigen::Affine3d& transform, const std::string& from_frame,
00059                                      const std::string& to_frame)
00060 {
00061   ROS_DEBUG_STREAM_NAMED("tf_visual_tools", "Publishing transform from " << from_frame << " to " << to_frame);
00062 
00063   // Create transform msg
00064   geometry_msgs::TransformStamped tf2_msg;
00065   tf2_msg.header.stamp = ros::Time::now();
00066   tf::transformEigenToMsg(transform, tf2_msg.transform);
00067   double quatNorm;
00068 
00069   // Normalizing the Quaternion
00070   quatNorm = 1 / sqrt(tf2_msg.transform.rotation.x * tf2_msg.transform.rotation.x +
00071                       tf2_msg.transform.rotation.y * tf2_msg.transform.rotation.y +
00072                       tf2_msg.transform.rotation.z * tf2_msg.transform.rotation.z +
00073                       tf2_msg.transform.rotation.w * tf2_msg.transform.rotation.w);
00074 
00075   tf2_msg.transform.rotation.x *= quatNorm;
00076   tf2_msg.transform.rotation.y *= quatNorm;
00077   tf2_msg.transform.rotation.z *= quatNorm;
00078   tf2_msg.transform.rotation.w *= quatNorm;
00079   tf2_msg.header.frame_id = from_frame;
00080   tf2_msg.child_frame_id = to_frame;
00081 
00082   // Check if this transform has already been added
00083   for (std::size_t i = 0; i < transforms_.size(); ++i)
00084   {
00085     if (transforms_[i].child_frame_id == to_frame && transforms_[i].header.frame_id == from_frame)
00086     {
00087       // ROS_WARN_STREAM_NAMED("tf_visual_tools", "This transform has already been added, updating");
00088       transforms_[i].transform = tf2_msg.transform;
00089       return true;
00090     }
00091   }
00092   // This transform is unique, add
00093   transforms_.push_back(tf2_msg);
00094 
00095   return true;
00096 }
00097 
00098 void TFVisualTools::publishAllTransforms(const ros::TimerEvent& e)
00099 {
00100   ROS_DEBUG_STREAM_NAMED("tf_visual_tools", "Publishing transforms");
00101 
00102   // Update timestamps
00103   for (std::size_t i = 0; i < transforms_.size(); ++i)
00104   {
00105     transforms_[i].header.stamp = ros::Time::now();
00106   }
00107   // Publish
00108   tf_pub_.sendTransform(transforms_);
00109 }
00110 
00111 }  // namespace rviz_visual_tools


rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 19:01:23