void RobotBase2DPoseDisplay::targetFrameChanged()
{
ROS_ASSERT( messages_.size() == arrows_.size() );
V_RobotBase2DOdom::iterator msg_it = messages_.begin();
V_Arrow::iterator arrow_it = arrows_.begin();
V_RobotBase2DOdom::iterator msg_end = messages_.end();
for ( ; msg_it != msg_end; ++msg_it, ++arrow_it )
{
transformArrow( *msg_it, *arrow_it );
}
}
void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
{
++messages_received_;
if( !validateFloats( *message ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
return;
}
setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
if( last_used_message_ )
{
Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
Ogre::Quaternion last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
(last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() )
{
return;
}
}
Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
transformArrow( message, arrow );
QColor color = color_property_->getColor();
arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
float length = length_property_->getFloat();
Ogre::Vector3 scale( length, length, length );
arrow->setScale( scale );
arrows_.push_back( arrow );
last_used_message_ = message;
context_->queueRender();
}
void RobotBase2DPoseDisplay::processMessage( const MessagePtr& message )
{
if ( last_used_message_ )
{
if ( abs(last_used_message_->pos.x - message->pos.x) < position_tolerance_
&& abs(last_used_message_->pos.y - message->pos.y) < position_tolerance_
&& abs(last_used_message_->pos.th - message->pos.th) < angle_tolerance_ )
{
return;
}
}
ogre_tools::Arrow* arrow = new ogre_tools::Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
transformArrow( message, arrow );
arrow->setColor( color_.r_, color_.g_, color_.b_, 1.0f );
arrow->setUserData( Ogre::Any((void*)this) );
arrows_.push_back( arrow );
last_used_message_ = message;
}
void RobotBase2DPoseDisplay::processMessage( const std_msgs::RobotBase2DOdom& message )
{
if ( !messages_.empty() )
{
const std_msgs::RobotBase2DOdom& last_message = messages_.back();
if ( abs(last_message.pos.x - message.pos.x) < position_tolerance_
&& abs(last_message.pos.y - message.pos.y) < position_tolerance_
&& abs(last_message.pos.th - message.pos.th) < angle_tolerance_ )
{
return;
}
}
ogre_tools::Arrow* arrow = new ogre_tools::Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
transformArrow( message, arrow );
arrow->setColor( color_.r_, color_.g_, color_.b_, 1.0f );
arrow->setUserData( Ogre::Any((void*)this) );
arrows_.push_back( arrow );
messages_.push_back( message );
}
