void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg)
{
if (!validateFloats(*msg)) {
setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)");
return;
}
latest_footstep_ = msg;
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if(!context_->getFrameManager()->getTransform( msg->header.frame_id,
msg->header.stamp,
position, orientation)) {
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
// check thhe length of the shapes_
allocateCubes(msg->footsteps.size());
allocateTexts(msg->footsteps.size());
line_->clear();
line_->setLineWidth(0.01);
line_->setNumLines(1);
line_->setMaxPointsPerLine(1024);
for (size_t i = 0; i < msg->footsteps.size(); i++)
{
ShapePtr shape = shapes_[i];
rviz::MovableText* text = texts_[i];
Ogre::SceneNode* node = text_nodes_[i];
jsk_footstep_msgs::Footstep footstep = msg->footsteps[i];
Ogre::Vector3 step_position;
Ogre::Quaternion step_quaternion;
if( !context_->getFrameManager()->transform( msg->header, footstep.pose,
step_position,
step_quaternion ))
{
ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
shape->setPosition(step_position);
shape->setOrientation(step_quaternion);
// size of shape
Ogre::Vector3 scale;
if (footstep.dimensions.x == 0 &&
footstep.dimensions.y == 0 &&
footstep.dimensions.z == 0) {
scale[0] = depth_;
scale[1] = width_;
scale[2] = height_;
}
else {
scale[0] = footstep.dimensions.x;
scale[1] = footstep.dimensions.y;
scale[2] = footstep.dimensions.z;
}
shape->setScale(scale);
// update the size of text
if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT) {
text->setCaption("L");
}
else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT) {
text->setCaption("R");
}
else {
text->setCaption("unknown");
}
text->setCharacterHeight(estimateTextSize(footstep));
node->setPosition(step_position);
node->setOrientation(step_quaternion);
text->setVisible(show_name_); // TODO
line_->addPoint(step_position);
}
//updateFootstepSize();
updateAlpha();
context_->queueRender();
}
void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg)
{
if (!validateFloats(*msg)) {
setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)");
return;
}
latest_footstep_ = msg;
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
msg->header.stamp,
position, orientation ))
{
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
// check thhe length of the shapes_
if (msg->footsteps.size() > shapes_.size())
{
// need to allocate
for (size_t i = shapes_.size(); i < msg->footsteps.size(); i++)
{
ShapePtr shape;
shape.reset(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(),
scene_node_));
shapes_.push_back(shape);
}
}
else if (msg->footsteps.size() < shapes_.size())
{
// need to remove
shapes_.resize(msg->footsteps.size());
}
line_->clear();
line_->setLineWidth(0.01);
line_->setNumLines(1);
line_->setMaxPointsPerLine(1024);
for (size_t i = 0; i < msg->footsteps.size(); i++)
{
ShapePtr shape = shapes_[i];
jsk_footstep_msgs::Footstep footstep = msg->footsteps[i];
Ogre::Vector3 step_position;
Ogre::Quaternion step_quaternion;
if( !context_->getFrameManager()->transform( msg->header, footstep.pose,
step_position,
step_quaternion ))
{
ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
return;
}
shape->setPosition(step_position);
shape->setOrientation(step_quaternion);
line_->addPoint(step_position);
}
updateFootstepSize();
updateAlpha();
context_->queueRender();
}