// This is our callback to handle an incoming message.
void MoveItCollisionMapDisplay::processMessage(const moveit_msgs::CollisionMap::ConstPtr& msg) {
// Here we call the rviz::FrameManager to get the transform from the
// fixed frame to the frame in the header of this Imu message. If
// it fails, we can't do anything else so we return.
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;
}
// We are keeping a circular buffer of visual pointers. This gets
// the next one, or creates and stores it if the buffer is not full
visuals_.clear();
boost::shared_ptr<MoveItCollisionMapVisual> visual;
vector<moveit_msgs::OrientedBoundingBox> vecBoxes = msg->boxes;
for(vector<moveit_msgs::OrientedBoundingBox>::iterator itBox = vecBoxes.begin();
itBox != vecBoxes.end();
itBox++) {
moveit_msgs::OrientedBoundingBox bxBox = *itBox;
const geometry_msgs::Point& ptPosition = bxBox.pose.position;
const geometry_msgs::Point32& ptExtents = bxBox.extents;
Ogre::Vector3 vecPosition(ptPosition.x, ptPosition.y, ptPosition.z);
Ogre::Vector3 vecExtents(ptExtents.x - 0.01, ptExtents.y - 0.01, ptExtents.z - 0.01);
//visual->setFramePosition(vecPosition);
//cube_->setPosition(vecPosition);
//cube_->setScale(vecExtents);
if(visuals_.full()) {
visual = visuals_.front();
} else {
visual.reset(new MoveItCollisionMapVisual(context_->getSceneManager(), scene_node_));
}
// Now set or update the contents of the chosen visual.
visual->setMessage(msg);
visual->setFramePosition(vecPosition);
visual->setExtents(vecExtents);
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual->setColor(color.r, color.g, color.b, alpha);
// And send it to the end of the circular buffer
// NOTE(winkler): Check here if this cube is already in the buffer.
visuals_.push_back(visual);
}
}
//-----------------------------------------------------------------------------
// Expand trigger bounds..
//-----------------------------------------------------------------------------
void CCollisionProperty::ComputeVPhysicsSurroundingBox( Vector *pVecWorldMins, Vector *pVecWorldMaxs )
{
bool bSetBounds = false;
IPhysicsObject *pPhysicsObject = GetOuter()->VPhysicsGetObject();
if ( pPhysicsObject )
{
if ( pPhysicsObject->GetCollide() )
{
physcollision->CollideGetAABB( pVecWorldMins, pVecWorldMaxs,
pPhysicsObject->GetCollide(), GetCollisionOrigin(), GetCollisionAngles() );
bSetBounds = true;
}
else if ( pPhysicsObject->GetSphereRadius( ) )
{
float flRadius = pPhysicsObject->GetSphereRadius( );
Vector vecExtents( flRadius, flRadius, flRadius );
VectorSubtract( GetCollisionOrigin(), vecExtents, *pVecWorldMins );
VectorAdd( GetCollisionOrigin(), vecExtents, *pVecWorldMaxs );
bSetBounds = true;
}
}
if ( !bSetBounds )
{
*pVecWorldMins = GetCollisionOrigin();
*pVecWorldMaxs = *pVecWorldMins;
}
// Also, lets expand for the trigger bounds also
if ( IsSolidFlagSet( FSOLID_USE_TRIGGER_BOUNDS ) )
{
Vector vecWorldTriggerMins, vecWorldTriggerMaxs;
WorldSpaceTriggerBounds( &vecWorldTriggerMins, &vecWorldTriggerMaxs );
VectorMin( vecWorldTriggerMins, *pVecWorldMins, *pVecWorldMins );
VectorMax( vecWorldTriggerMaxs, *pVecWorldMaxs, *pVecWorldMaxs );
}
}
