void jsk_pcl_ros::DepthImageCreator::onInit () {
JSK_NODELET_INFO("[%s::onInit]", getName().c_str());
ConnectionBasedNodelet::onInit();
tf_listener_ = TfListenerSingleton::getInstance();
// scale_depth
pnh_->param("scale_depth", scale_depth, 1.0);
JSK_ROS_INFO("scale_depth : %f", scale_depth);
// use fixed transform
pnh_->param("use_fixed_transform", use_fixed_transform, false);
JSK_ROS_INFO("use_fixed_transform : %d", use_fixed_transform);
pnh_->param("use_service", use_service, false);
JSK_ROS_INFO("use_service : %d", use_service);
pnh_->param("use_asynchronous", use_asynchronous, false);
JSK_ROS_INFO("use_asynchronous : %d", use_asynchronous);
pnh_->param("use_approximate", use_approximate, false);
JSK_ROS_INFO("use_approximate : %d", use_approximate);
pnh_->param("info_throttle", info_throttle_, 0);
info_counter_ = 0;
pnh_->param("max_queue_size", max_queue_size_, 3);
// set transformation
std::vector<double> trans_pos(3, 0);
std::vector<double> trans_quat(4, 0); trans_quat[3] = 1.0;
if (pnh_->hasParam("translation")) {
jsk_topic_tools::readVectorParameter(*pnh_, "translation", trans_pos);
}
if (pnh_->hasParam("rotation")) {
jsk_topic_tools::readVectorParameter(*pnh_, "rotation", trans_quat);
}
tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
fixed_transform.setOrigin(btp);
fixed_transform.setRotation(btq);
pub_image_ = advertise<sensor_msgs::Image> (*pnh_, "output", max_queue_size_);
pub_cloud_ = advertise<PointCloud>(*pnh_, "output_cloud", max_queue_size_);
pub_disp_image_ = advertise<stereo_msgs::DisparityImage> (*pnh_, "output_disp", max_queue_size_);
if (use_service) {
service_ = pnh_->advertiseService("set_point_cloud",
&DepthImageCreator::service_cb, this);
}
onInitPostProcess();
}
void EyeContact::GetRPY(const geometry_msgs::Quaternion &q,double &roll,double &pitch,double &yaw)
{
tf::Quaternion btq(q.x,q.y,q.z,q.w);
tf::Matrix3x3(btq).getRPY(roll, pitch, yaw);
}
