void FootstepDisplay::allocateCubes(size_t num) {
if (num > shapes_.size()) {
// need to allocate
for (size_t i = shapes_.size(); i < num; i++) {
ShapePtr shape;
shape.reset(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(),
scene_node_));
shapes_.push_back(shape);
}
}
else if (num < shapes_.size()) {
// need to remove
shapes_.resize(num);
}
}
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();
}