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;
}
Exemple #2
0
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;
  }