CameraFactory::CameraFactory() :
camera_(0)
{
initialized_ = (openni::OpenNI::initialize() == openni::STATUS_OK);
ROS_ERROR_COND(!initialized_, "OpenNI2 initialization failed!");
}
bool readDoubleArray(ros::NodeHandle& node_handle, const std::string& parameter_name, std::vector<double>& array, const bool verbose)
{
XmlRpc::XmlRpcValue d_array_xml;
if(!node_handle.getParam(parameter_name, d_array_xml))
{
ROS_ERROR_COND(verbose, "Could not retrieve parameter %s in namespace %s.", parameter_name.c_str(), node_handle.getNamespace().c_str());
return false;
}
if (d_array_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR_COND(verbose, "XmlRpcValue is not of type array.");
return false;
}
array.clear();
for (int i=0; i<d_array_xml.size(); ++i)
{
if (d_array_xml[i].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
d_array_xml[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
{
ROS_ERROR_COND(verbose, "XmlRpcValue is neither a double nor a integer array.");
return false;
}
double value = 0.0;
if (d_array_xml[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
{
value = static_cast<double>(static_cast<int>(d_array_xml[i]));
}
else
{
value = static_cast<double>(d_array_xml[i]);
}
array.push_back(value);
}
return true;
}
bool readStringArray(ros::NodeHandle& node_handle, const std::string& parameter_name, std::vector<std::string>& str_array, const bool verbose)
{
XmlRpc::XmlRpcValue list;
if (!node_handle.getParam(parameter_name, list))
{
ROS_ERROR_COND(verbose, "Could not retrieve parameter %s in namespace %s.", parameter_name.c_str(), node_handle.getNamespace().c_str());
return false;
}
XmlRpc::XmlRpcValue str_array_xml = list;
if (str_array_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR_COND(verbose, "XmlRpcValue is not of type array.");
return false;
}
str_array.clear();
for (int i = 0; i < str_array_xml.size(); ++i)
{
str_array.push_back(std::string(str_array_xml[i]));
}
return true;
}
void CameraNodelet::onInit()
{
ROS_ERROR_COND(!camera_factory_.create(getNodeHandle(), getPrivateNodeHandle(), "#1"), "Failed to open camera!");
}
void PointCloudDisplay::transformCloud()
{
if ( message_.header.frame_id.empty() )
{
message_.header.frame_id = fixed_frame_;
}
tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0, 0, 0 ), btVector3( 0, 0, 0 ) ), message_.header.stamp, message_.header.frame_id );
try
{
tf_->transformPose( fixed_frame_, pose, pose );
}
catch(tf::TransformException& e)
{
ROS_ERROR( "Error transforming point cloud '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message_.header.frame_id.c_str(), fixed_frame_.c_str() );
}
Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
robotToOgre( position );
btScalar yaw, pitch, roll;
pose.getBasis().getEulerZYX( yaw, pitch, roll );
Ogre::Matrix3 orientation( ogreMatrixFromRobotEulers( yaw, pitch, roll ) );
// First find the min/max intensity values
float min_intensity = 999999.0f;
float max_intensity = -999999.0f;
typedef std::vector<std_msgs::ChannelFloat32> V_Chan;
typedef std::vector<bool> V_bool;
V_bool valid_channels(message_.chan.size());
uint32_t point_count = message_.get_pts_size();
V_Chan::iterator chan_it = message_.chan.begin();
V_Chan::iterator chan_end = message_.chan.end();
uint32_t index = 0;
for ( ; chan_it != chan_end; ++chan_it, ++index )
{
std_msgs::ChannelFloat32& chan = *chan_it;
uint32_t val_count = chan.vals.size();
bool channel_size_correct = val_count == point_count;
ROS_ERROR_COND(!channel_size_correct, "Point cloud '%s' on topic '%s' has channel 0 with fewer values than points (%d values, %d points)", name_.c_str(), topic_.c_str(), val_count, point_count);
valid_channels[index] = channel_size_correct;
if ( channel_size_correct && ( chan.name.empty() || chan.name == "intensity" || chan.name == "intensities" ) )
{
for(uint32_t i = 0; i < point_count; i++)
{
float& intensity = chan.vals[i];
// arbitrarily cap to 4096 for now
intensity = std::min( intensity, 4096.0f );
min_intensity = std::min( min_intensity, intensity );
max_intensity = std::max( max_intensity, intensity );
}
}
}
float diff_intensity = max_intensity - min_intensity;
typedef std::vector< ogre_tools::PointCloud::Point > V_Point;
V_Point points;
points.resize( point_count );
for(uint32_t i = 0; i < point_count; i++)
{
Ogre::Vector3 color( color_.r_, color_.g_, color_.b_ );
ogre_tools::PointCloud::Point& current_point = points[ i ];
current_point.x_ = message_.pts[i].x;
current_point.y_ = message_.pts[i].y;
current_point.z_ = message_.pts[i].z;
current_point.r_ = color.x;
current_point.g_ = color.y;
current_point.b_ = color.z;
}
chan_it = message_.chan.begin();
index = 0;
for ( ; chan_it != chan_end; ++chan_it, ++index )
{
if ( !valid_channels[index] )
{
continue;
}
std_msgs::ChannelFloat32& chan = *chan_it;
enum ChannelType
{
CT_INTENSITY,
CT_RGB,
CT_R,
CT_G,
CT_B,
CT_COUNT
};
ChannelType type = CT_INTENSITY;
if ( chan.name == "rgb" )
{
type = CT_RGB;
}
else if ( chan.name == "r" )
{
type = CT_R;
}
else if ( chan.name == "g" )
{
type = CT_G;
}
else if ( chan.name == "b" )
{
type = CT_B;
}
typedef void (*TransformFunc)(float, ogre_tools::PointCloud::Point&, float, float, float);
TransformFunc funcs[CT_COUNT] =
{
transformIntensity,
transformRGB,
transformR,
transformG,
transformB
};
for(uint32_t i = 0; i < point_count; i++)
{
ogre_tools::PointCloud::Point& current_point = points[ i ];
funcs[type]( chan.vals[i], current_point, min_intensity, max_intensity, diff_intensity );
}
}
{
RenderAutoLock renderLock( this );
scene_node_->setPosition( position );
scene_node_->setOrientation( orientation );
cloud_->clear();
if ( !points.empty() )
{
cloud_->addPoints( &points.front(), points.size() );
}
}
causeRender();
}
