message_context.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  * Author: David Gossow
00030  */
00031 
00032 #include "interactive_markers/detail/message_context.h"
00033 #include "interactive_markers/tools.h"
00034 
00035 #include <boost/make_shared.hpp>
00036 
00037 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ );
00038 //#define DBG_MSG( ... ) printf("   "); printf( __VA_ARGS__ ); printf("\n");
00039 
00040 namespace interactive_markers
00041 {
00042 
00043 template<class MsgT>
00044 MessageContext<MsgT>::MessageContext(
00045     tf::Transformer& tf,
00046     const std::string& target_frame,
00047     const typename MsgT::ConstPtr& _msg,
00048     bool enable_autocomplete_transparency)
00049 : tf_(tf)
00050 , target_frame_(target_frame)
00051 , enable_autocomplete_transparency_(enable_autocomplete_transparency)
00052 {
00053   // copy message, as we will be modifying it
00054   msg = boost::make_shared<MsgT>( *_msg );
00055 
00056   init();
00057 }
00058 
00059 template<class MsgT>
00060 MessageContext<MsgT>& MessageContext<MsgT>::operator=( const MessageContext<MsgT>& other )
00061 {
00062   open_marker_idx_ = other.open_marker_idx_;
00063   open_pose_idx_ = other.open_pose_idx_;
00064   target_frame_ = other.target_frame_;
00065   enable_autocomplete_transparency_ = other.enable_autocomplete_transparency_;
00066   return *this;
00067 }
00068 
00069 template<class MsgT>
00070 bool MessageContext<MsgT>::getTransform( std_msgs::Header& header, geometry_msgs::Pose& pose_msg )
00071 {
00072   try
00073   {
00074     if ( header.frame_id != target_frame_ )
00075     {
00076       // get transform
00077       tf::StampedTransform transform;
00078       tf_.lookupTransform( target_frame_, header.frame_id, header.stamp, transform );
00079       DBG_MSG( "Transform %s -> %s at time %f is ready.", header.frame_id.c_str(), target_frame_.c_str(), header.stamp.toSec() );
00080 
00081       // if timestamp is given, transform message into target frame
00082       if ( header.stamp != ros::Time(0) )
00083       {
00084         tf::Pose pose;
00085         tf::poseMsgToTF( pose_msg, pose );
00086         pose = transform * pose;
00087         // store transformed pose in original message
00088         tf::poseTFToMsg( pose, pose_msg );
00089         ROS_DEBUG_STREAM("Changing " << header.frame_id << " to "<< target_frame_);
00090         header.frame_id = target_frame_;
00091       }
00092     }
00093   }
00094   catch ( tf::ExtrapolationException& e )
00095   {
00096     ros::Time latest_time;
00097     std::string error_string;
00098 
00099     tf_.getLatestCommonTime( target_frame_, header.frame_id, latest_time, &error_string );
00100 
00101     // if we have some tf info and it is newer than the requested time,
00102     // we are very unlikely to ever receive the old tf info in the future.
00103     if ( latest_time != ros::Time(0) && latest_time > header.stamp )
00104     {
00105       std::ostringstream s;
00106       s << "The init message contains an old timestamp and cannot be transformed ";
00107       s << "('" << header.frame_id << "' to '" << target_frame_
00108         << "' at time " << header.stamp << ").";
00109       throw InitFailException( s.str() );
00110     }
00111     return false;
00112   }
00113   return true;
00114   // all other exceptions need to be handled outside
00115 }
00116 
00117 template<class MsgT>
00118 void MessageContext<MsgT>::getTfTransforms( std::vector<visualization_msgs::InteractiveMarker>& msg_vec, std::list<size_t>& indices )
00119 {
00120   std::list<size_t>::iterator idx_it;
00121   for ( idx_it = indices.begin(); idx_it != indices.end(); )
00122   {
00123     visualization_msgs::InteractiveMarker& im_msg = msg_vec[ *idx_it ];
00124     // transform interactive marker
00125     bool success = getTransform( im_msg.header, im_msg.pose );
00126     // transform regular markers
00127     for ( unsigned c = 0; c<im_msg.controls.size(); c++ )
00128     {
00129       visualization_msgs::InteractiveMarkerControl& ctrl_msg = im_msg.controls[c];
00130       for ( unsigned m = 0; m<ctrl_msg.markers.size(); m++ )
00131       {
00132         visualization_msgs::Marker& marker_msg = ctrl_msg.markers[m];
00133         if ( !marker_msg.header.frame_id.empty() ) {
00134           success = success && getTransform( marker_msg.header, marker_msg.pose );
00135         }
00136       }
00137     }
00138 
00139     if ( success )
00140     {
00141       idx_it = indices.erase(idx_it);
00142     }
00143     else
00144     {
00145       DBG_MSG( "Transform %s -> %s at time %f is not ready.", im_msg.header.frame_id.c_str(), target_frame_.c_str(), im_msg.header.stamp.toSec() );
00146       ++idx_it;
00147     }
00148   }
00149 }
00150 
00151 template<class MsgT>
00152 void MessageContext<MsgT>::getTfTransforms( std::vector<visualization_msgs::InteractiveMarkerPose>& msg_vec, std::list<size_t>& indices )
00153 {
00154   std::list<size_t>::iterator idx_it;
00155   for ( idx_it = indices.begin(); idx_it != indices.end(); )
00156   {
00157     visualization_msgs::InteractiveMarkerPose& msg = msg_vec[ *idx_it ];
00158     if ( getTransform( msg.header, msg.pose ) )
00159     {
00160       idx_it = indices.erase(idx_it);
00161     }
00162     else
00163     {
00164       DBG_MSG( "Transform %s -> %s at time %f is not ready.", msg.header.frame_id.c_str(), target_frame_.c_str(), msg.header.stamp.toSec() );
00165       ++idx_it;
00166     }
00167   }
00168 }
00169 
00170 template<class MsgT>
00171 bool MessageContext<MsgT>::isReady()
00172 {
00173   return open_marker_idx_.empty() && open_pose_idx_.empty();
00174 }
00175 
00176 template<>
00177 void MessageContext<visualization_msgs::InteractiveMarkerUpdate>::init()
00178 {
00179   // mark all transforms as being missing
00180   for ( size_t i=0; i<msg->markers.size(); i++ )
00181   {
00182     open_marker_idx_.push_back( i );
00183   }
00184   for ( size_t i=0; i<msg->poses.size(); i++ )
00185   {
00186     open_pose_idx_.push_back( i );
00187   }
00188   for( unsigned i=0; i<msg->markers.size(); i++ )
00189   {
00190     autoComplete( msg->markers[i], enable_autocomplete_transparency_  );
00191   }
00192   for( unsigned i=0; i<msg->poses.size(); i++ )
00193   {
00194     // correct empty orientation
00195     if ( msg->poses[i].pose.orientation.w == 0 && msg->poses[i].pose.orientation.x == 0 &&
00196         msg->poses[i].pose.orientation.y == 0 && msg->poses[i].pose.orientation.z == 0 )
00197     {
00198       msg->poses[i].pose.orientation.w = 1;
00199     }
00200   }
00201 }
00202 
00203 template<>
00204 void MessageContext<visualization_msgs::InteractiveMarkerInit>::init()
00205 {
00206   // mark all transforms as being missing
00207   for ( size_t i=0; i<msg->markers.size(); i++ )
00208   {
00209     open_marker_idx_.push_back( i );
00210   }
00211   for( unsigned i=0; i<msg->markers.size(); i++ )
00212   {
00213     autoComplete( msg->markers[i], enable_autocomplete_transparency_ );
00214   }
00215 }
00216 
00217 template<>
00218 void MessageContext<visualization_msgs::InteractiveMarkerUpdate>::getTfTransforms( )
00219 {
00220   getTfTransforms( msg->markers, open_marker_idx_ );
00221   getTfTransforms( msg->poses, open_pose_idx_ );
00222   if ( isReady() )
00223   {
00224     DBG_MSG( "Update message with seq_num=%lu is ready.", msg->seq_num );
00225   }
00226 }
00227 
00228 template<>
00229 void MessageContext<visualization_msgs::InteractiveMarkerInit>::getTfTransforms( )
00230 {
00231   getTfTransforms( msg->markers, open_marker_idx_ );
00232   if ( isReady() )
00233   {
00234     DBG_MSG( "Init message with seq_num=%lu is ready.", msg->seq_num );
00235   }
00236 }
00237 
00238 // explicit template instantiation
00239 template class MessageContext<visualization_msgs::InteractiveMarkerUpdate>;
00240 template class MessageContext<visualization_msgs::InteractiveMarkerInit>;
00241 
00242 
00243 }
00244 
00245 


interactive_markers
Author(s): David Gossow
autogenerated on Fri Aug 28 2015 11:11:26