35 #include <boost/make_shared.hpp> 37 #define DBG_MSG( ... ) ROS_DEBUG( __VA_ARGS__ ); 47 const typename MsgT::ConstPtr& _msg,
48 bool enable_autocomplete_transparency)
50 , target_frame_(target_frame)
51 , enable_autocomplete_transparency_(enable_autocomplete_transparency)
54 msg = boost::make_shared<MsgT>( *_msg );
79 DBG_MSG(
"Transform %s -> %s at time %f is ready.", header.frame_id.c_str(),
target_frame_.c_str(), header.stamp.toSec() );
86 pose = transform * pose;
97 std::string error_string;
103 if ( latest_time !=
ros::Time(0) && latest_time > header.stamp )
105 std::ostringstream
s;
106 s <<
"The init message contains an old timestamp and cannot be transformed ";
108 <<
"' at time " << header.stamp <<
").";
120 std::list<size_t>::iterator idx_it;
121 for ( idx_it = indices.begin(); idx_it != indices.end(); )
123 visualization_msgs::InteractiveMarker& im_msg = msg_vec[ *idx_it ];
125 bool success =
getTransform( im_msg.header, im_msg.pose );
127 for (
unsigned c = 0; c<im_msg.controls.size(); c++ )
129 visualization_msgs::InteractiveMarkerControl& ctrl_msg = im_msg.controls[c];
130 for (
unsigned m = 0; m<ctrl_msg.markers.size(); m++ )
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 );
141 idx_it = indices.erase(idx_it);
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() );
154 std::list<size_t>::iterator idx_it;
155 for ( idx_it = indices.begin(); idx_it != indices.end(); )
157 visualization_msgs::InteractiveMarkerPose&
msg = msg_vec[ *idx_it ];
160 idx_it = indices.erase(idx_it);
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() );
180 for (
size_t i=0; i<
msg->markers.size(); i++ )
184 for (
size_t i=0; i<
msg->poses.size(); i++ )
188 for(
unsigned i=0; i<
msg->markers.size(); i++ )
192 for(
unsigned i=0; i<
msg->poses.size(); i++ )
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 )
198 msg->poses[i].pose.orientation.w = 1;
207 for (
size_t i=0; i<
msg->markers.size(); i++ )
211 for(
unsigned i=0; i<
msg->markers.size(); i++ )
224 DBG_MSG(
"Update message with seq_num=%lu is ready.",
msg->seq_num );
234 DBG_MSG(
"Init message with seq_num=%lu is ready.",
msg->seq_num );
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
std::list< size_t > open_marker_idx_
MessageContext(tf::Transformer &tf, const std::string &target_frame, const typename MsgT::ConstPtr &msg, bool enable_autocomplete_transparency=true)
INTERACTIVE_MARKERS_PUBLIC void autoComplete(visualization_msgs::InteractiveMarker &msg, bool enable_autocomplete_transparency=true)
fill in default values & insert default controls when none are specified.
MessageContext< MsgT > & operator=(const MessageContext< MsgT > &other)
std::list< size_t > open_pose_idx_
bool getTransform(std_msgs::Header &header, geometry_msgs::Pose &pose_msg)
#define ROS_DEBUG_STREAM(args)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
bool enable_autocomplete_transparency_
std::string target_frame_