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; }