Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
// TF ready?
Transform transform;
try
{
if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
{
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
{
ROS_WARN("odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_);
return transform;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
transform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
}
return transform;
}
void updateControl(const ros::TimerEvent& event) {
if (!controller.enabled)
return;
float dt = controller.command.period;
if (!lastTime.isZero())
dt = (event.current_expected-lastTime).toSec();
if ((dt >= 0.5f*controller.command.period) && controller.high) {
FadeToColor fadeToColor;
float color[] = {0.0f, 0.0f, 0.0f};
copy(color, fadeToColor.request.rgb);
fadeToColor.request.speed = 1.0f/controllerFrequency;
if (fadeToColorClient.call(fadeToColor)) {
controller.high = false;
diagnoseFrequency->tick();
}
}
else if ((dt >= controller.command.period) && !controller.high) {
FadeToColor fadeToColor;
copy(controller.command.color, fadeToColor.request.rgb);
fadeToColor.request.speed = 1.0f/controllerFrequency;
if (fadeToColorClient.call(fadeToColor)) {
controller.high = true;
lastTime = event.current_expected;
diagnoseFrequency->tick();
}
}
else
diagnoseFrequency->tick();
}
bool processFrame(tPvFrame* frame, sensor_msgs::Image &img, sensor_msgs::CameraInfo &cam_info)
{
/// @todo Better trigger timestamp matching
if ((trigger_mode_ == prosilica::SyncIn1 || trigger_mode_ == prosilica::SyncIn2) && !trig_time_.isZero()) {
img.header.stamp = cam_info.header.stamp = trig_time_;
trig_time_ = ros::Time(); // Zero
}
else {
/// @todo Match time stamp from frame to ROS time?
img.header.stamp = cam_info.header.stamp = ros::Time::now();
}
/// @todo Binning values retrieved here may differ from the ones used to actually
/// capture the frame! Maybe need to clear queue when changing binning and/or
/// stuff binning values into context?
tPvUint32 binning_x = 1, binning_y = 1;
if (cam_->hasAttribute("BinningX")) {
cam_->getAttribute("BinningX", binning_x);
cam_->getAttribute("BinningY", binning_y);
}
// Binning averages bayer samples, so just call it mono8 in that case
if (frame->Format == ePvFmtBayer8 && (binning_x > 1 || binning_y > 1))
frame->Format = ePvFmtMono8;
if (!frameToImage(frame, img))
return false;
// Set the operational parameters in CameraInfo (binning, ROI)
cam_info.binning_x = binning_x;
cam_info.binning_y = binning_y;
// ROI in CameraInfo is in unbinned coordinates, need to scale up
cam_info.roi.x_offset = frame->RegionX * binning_x;
cam_info.roi.y_offset = frame->RegionY * binning_y;
cam_info.roi.height = frame->Height * binning_y;
cam_info.roi.width = frame->Width * binning_x;
cam_info.roi.do_rectify = (frame->Height != sensor_height_ / binning_y) ||
(frame->Width != sensor_width_ / binning_x);
count_++;
return true;
}
