message_context.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Author: David Gossow
30  */
31 
34 
35 #include <boost/make_shared.hpp>
36 
37 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ );
38 //#define DBG_MSG( ... ) printf(" "); printf( __VA_ARGS__ ); printf("\n");
39 
40 namespace interactive_markers
41 {
42 
43 template<class MsgT>
46  const std::string& target_frame,
47  const typename MsgT::ConstPtr& _msg,
48  bool enable_autocomplete_transparency)
49 : tf_(tf)
50 , target_frame_(target_frame)
51 , enable_autocomplete_transparency_(enable_autocomplete_transparency)
52 {
53  // copy message, as we will be modifying it
54  msg = boost::make_shared<MsgT>( *_msg );
55 
56  init();
57 }
58 
59 template<class MsgT>
61 {
66  return *this;
67 }
68 
69 template<class MsgT>
70 bool MessageContext<MsgT>::getTransform( std_msgs::Header& header, geometry_msgs::Pose& pose_msg )
71 {
72  try
73  {
74  if ( header.frame_id != target_frame_ )
75  {
76  // get transform
77  tf::StampedTransform transform;
78  tf_.lookupTransform( target_frame_, header.frame_id, header.stamp, transform );
79  DBG_MSG( "Transform %s -> %s at time %f is ready.", header.frame_id.c_str(), target_frame_.c_str(), header.stamp.toSec() );
80 
81  // if timestamp is given, transform message into target frame
82  if ( header.stamp != ros::Time(0) )
83  {
84  tf::Pose pose;
85  tf::poseMsgToTF( pose_msg, pose );
86  pose = transform * pose;
87  // store transformed pose in original message
88  tf::poseTFToMsg( pose, pose_msg );
89  ROS_DEBUG_STREAM("Changing " << header.frame_id << " to "<< target_frame_);
90  header.frame_id = target_frame_;
91  }
92  }
93  }
94  catch ( tf::ExtrapolationException& e )
95  {
96  ros::Time latest_time;
97  std::string error_string;
98 
99  tf_.getLatestCommonTime( target_frame_, header.frame_id, latest_time, &error_string );
100 
101  // if we have some tf info and it is newer than the requested time,
102  // we are very unlikely to ever receive the old tf info in the future.
103  if ( latest_time != ros::Time(0) && latest_time > header.stamp )
104  {
105  std::ostringstream s;
106  s << "The init message contains an old timestamp and cannot be transformed ";
107  s << "('" << header.frame_id << "' to '" << target_frame_
108  << "' at time " << header.stamp << ").";
109  throw InitFailException( s.str() );
110  }
111  return false;
112  }
113  return true;
114  // all other exceptions need to be handled outside
115 }
116 
117 template<class MsgT>
118 void MessageContext<MsgT>::getTfTransforms( std::vector<visualization_msgs::InteractiveMarker>& msg_vec, std::list<size_t>& indices )
119 {
120  std::list<size_t>::iterator idx_it;
121  for ( idx_it = indices.begin(); idx_it != indices.end(); )
122  {
123  visualization_msgs::InteractiveMarker& im_msg = msg_vec[ *idx_it ];
124  // transform interactive marker
125  bool success = getTransform( im_msg.header, im_msg.pose );
126  // transform regular markers
127  for ( unsigned c = 0; c<im_msg.controls.size(); c++ )
128  {
129  visualization_msgs::InteractiveMarkerControl& ctrl_msg = im_msg.controls[c];
130  for ( unsigned m = 0; m<ctrl_msg.markers.size(); m++ )
131  {
132  visualization_msgs::Marker& marker_msg = ctrl_msg.markers[m];
133  if ( !marker_msg.header.frame_id.empty() ) {
134  success = success && getTransform( marker_msg.header, marker_msg.pose );
135  }
136  }
137  }
138 
139  if ( success )
140  {
141  idx_it = indices.erase(idx_it);
142  }
143  else
144  {
145  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() );
146  ++idx_it;
147  }
148  }
149 }
150 
151 template<class MsgT>
152 void MessageContext<MsgT>::getTfTransforms( std::vector<visualization_msgs::InteractiveMarkerPose>& msg_vec, std::list<size_t>& indices )
153 {
154  std::list<size_t>::iterator idx_it;
155  for ( idx_it = indices.begin(); idx_it != indices.end(); )
156  {
157  visualization_msgs::InteractiveMarkerPose& msg = msg_vec[ *idx_it ];
158  if ( getTransform( msg.header, msg.pose ) )
159  {
160  idx_it = indices.erase(idx_it);
161  }
162  else
163  {
164  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() );
165  ++idx_it;
166  }
167  }
168 }
169 
170 template<class MsgT>
172 {
173  return open_marker_idx_.empty() && open_pose_idx_.empty();
174 }
175 
176 template<>
178 {
179  // mark all transforms as being missing
180  for ( size_t i=0; i<msg->markers.size(); i++ )
181  {
182  open_marker_idx_.push_back( i );
183  }
184  for ( size_t i=0; i<msg->poses.size(); i++ )
185  {
186  open_pose_idx_.push_back( i );
187  }
188  for( unsigned i=0; i<msg->markers.size(); i++ )
189  {
191  }
192  for( unsigned i=0; i<msg->poses.size(); i++ )
193  {
194  // correct empty orientation
195  if ( msg->poses[i].pose.orientation.w == 0 && msg->poses[i].pose.orientation.x == 0 &&
196  msg->poses[i].pose.orientation.y == 0 && msg->poses[i].pose.orientation.z == 0 )
197  {
198  msg->poses[i].pose.orientation.w = 1;
199  }
200  }
201 }
202 
203 template<>
205 {
206  // mark all transforms as being missing
207  for ( size_t i=0; i<msg->markers.size(); i++ )
208  {
209  open_marker_idx_.push_back( i );
210  }
211  for( unsigned i=0; i<msg->markers.size(); i++ )
212  {
214  }
215 }
216 
217 template<>
219 {
220  getTfTransforms( msg->markers, open_marker_idx_ );
222  if ( isReady() )
223  {
224  DBG_MSG( "Update message with seq_num=%lu is ready.", msg->seq_num );
225  }
226 }
227 
228 template<>
230 {
231  getTfTransforms( msg->markers, open_marker_idx_ );
232  if ( isReady() )
233  {
234  DBG_MSG( "Init message with seq_num=%lu is ready.", msg->seq_num );
235  }
236 }
237 
238 // explicit template instantiation
241 
242 
243 }
244 
245 
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
std::string target_frame
Definition: client_test.cpp:74
XmlRpcServer s
int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const
MessageContext(tf::Transformer &tf, const std::string &target_frame, const typename MsgT::ConstPtr &msg, bool enable_autocomplete_transparency=true)
void autoComplete(visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency=true)
fill in default values & insert default controls when none are specified.
Definition: tools.cpp:44
MessageContext< MsgT > & operator=(const MessageContext< MsgT > &other)
bool getTransform(std_msgs::Header &header, geometry_msgs::Pose &pose_msg)
#define ROS_DEBUG_STREAM(args)
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
#define DBG_MSG(...)


interactive_markers
Author(s): David Gossow
autogenerated on Sun Feb 3 2019 03:24:09