SwissRangerDevice::Data::Ptr SwissRangerDevice::convertMessages(const Messages & messages) { assert(messages.cloud_msg and messages.intensity_msg); cv_bridge::CvImage::Ptr image_ptr = cv_bridge::toCvCopy(messages.intensity_msg, sensor_msgs::image_encodings::MONO8); Data::Ptr data = boost::make_shared<Data>(); try { cv::Mat rectified; intensity_sensor_->cameraModel()->rectifyImage(image_ptr->image, rectified); data->intensity_image = rectified; } catch (const image_geometry::Exception & ex) { ROS_WARN_STREAM_ONCE(ex.what() << std::endl << "Message (from topic \"" << camera_sub_.getTopic() << "\"): " << std::endl << intensity_sensor_->cameraModel()->cameraInfo()); data->intensity_image = image_ptr->image; } sr::Utility sr_utility; sr_utility.setConfidenceThreshold(confidence_threshold_); sr_utility.setInputCloud(messages.cloud_msg); sr_utility.setIntensityType(sr::Utility::INTENSITY_8BIT); sr_utility.setNormalizeIntensity(true); sr_utility.split(sr::Utility::CLOUD | sr::Utility::INTENSITY); data->cloud = sr_utility.cloud(); return data; }
void MeshPersonVisual::setAnimationState(const std::string& nameOfAnimationState) { Ogre::AnimationStateSet *animationStates = entity_->getAllAnimationStates(); if(animationStates != NULL) { Ogre::AnimationStateIterator animationsIterator = animationStates->getAnimationStateIterator(); while (animationsIterator.hasMoreElements()) { Ogre::AnimationState *animationState = animationsIterator.getNext(); if(animationState->getAnimationName() == nameOfAnimationState || nameOfAnimationState.empty()) { animationState->setLoop(true); animationState->setEnabled(true); m_animationState = animationState; return; } } // Not found. Set first animation state then. ROS_WARN_STREAM_ONCE("Person mesh animation state " << nameOfAnimationState << " does not exist in mesh!"); setAnimationState(""); } }
PinholeRGBDevice::Data::Ptr PinholeRGBDevice::convertMessages(const Messages & messages) { assert(sensor_); cv_bridge::CvImage::Ptr image_ptr = cv_bridge::toCvCopy(messages.image_msg, sensor_msgs::image_encodings::BGR8); Data::Ptr data = boost::make_shared<Data>(); try { cv::Mat rectified; sensor_->cameraModel()->rectifyImage(image_ptr->image, rectified); data->image = rectified; } catch (const image_geometry::Exception & ex) { ROS_WARN_STREAM_ONCE(ex.what() << std::endl << "Message (from topic \"" << image_sub_.getTopic() << "\"): " << std::endl << sensor_->cameraModel()->cameraInfo()); data->image = image_ptr->image; } return data; }
KinectDevice::Data::Ptr KinectDevice::convertMessages(const Messages & messages) { assert(color_sensor_); cv_bridge::CvImage::Ptr image_ptr = cv_bridge::toCvCopy(messages.image_msg, sensor_msgs::image_encodings::BGR8); Data::Ptr data = boost::make_shared<Data>(); try { cv::Mat rectified; color_sensor_->cameraModel()->rectifyImage(image_ptr->image, rectified); data->image = rectified; } catch (const image_geometry::Exception & ex) { ROS_WARN_STREAM_ONCE(ex.what() << std::endl << "Message (from topic \"" << camera_sub_.getTopic() << "\"): " << std::endl << color_sensor_->cameraModel()->cameraInfo()); data->image = image_ptr->image; } data->cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(); pcl::fromROSMsg(*messages.cloud_msg, *data->cloud); return data; }