// What we want to do here is to look at all the points that are *not* included in the indices list, // (i.e. these are the outliers, the points not on the floor) and determine which are the closest to the camera void cloudIndicesModelCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, const PointIndicesConstPtr& indices, const pcl::ModelCoefficientsConstPtr& model) { boost::mutex::scoped_lock lock (callback_mutex); NODELET_DEBUG_STREAM("Got cloud with timestamp " << cloud_msg->header.stamp << " + indices with timestamp " << indices->header.stamp << " + model with timestamp " << model->header.stamp); double dt; if (first) { first = false; dt = 0; } else { dt = (cloud_msg->header.stamp - prev_cloud_time).toSec(); prev_cloud_time = cloud_msg->header.stamp; } if (model->values.size() > 0) { // Determine altitude: kinect_z = reject_outliers(-fabs(model->values[3])) - imu_to_kinect_offset; calcVelocity(kinect_z, dt, kinect_vz); // Detect obstacles: pcl::PointXYZ obstacle_location; bool obstacle_found = detectObstacle(cloud_msg, indices, model, obstacle_location); if (obstacle_found) { publishObstacleMarker(obstacle_location); NODELET_DEBUG("Detected obstacle at: [%f, %f, %f]", obstacle_location.x, obstacle_location.y, obstacle_location.z); } publishObstacle(obstacle_found, obstacle_location); } else { NODELET_WARN("No planar model found -- cannot determine altitude or obstacles"); } updateMask(); if (not use_backup_estimator_alt) { publishOdom(); } if (debug_throttle_rate > 0) { ros::Duration(1 / debug_throttle_rate).sleep(); } }
void FileSeqPubCore::process() { if (one_frame_ == -1) //play back set of frames { step_++; } else //play back only one chosen frame { step_ = one_frame_; if (step_ > (int)temp_trajectory_.size()) { ROS_ERROR("Chosen frame is not exist!"); ros::shutdown(); } } if (step_ < (int)temp_trajectory_.size()) //process step { ROS_INFO("---------------Processing step %d...---------------\n", step_); acquirePose(); acquireImage(); publishOdom(); publishImage(); } else //if end of loaded data { if (loop_f_) //if want to play from begginning { ROS_INFO("----Start from the beginning...----\n"); step_ = -1; } else //end of publishing { ROS_INFO("----Whole trajectory was published. Quitting...----\n"); ros::shutdown(); } } }
void VisualOdometry::RGBDCallback( const ImageMsg::ConstPtr& rgb_msg, const ImageMsg::ConstPtr& depth_msg, const CameraInfoMsg::ConstPtr& info_msg) { ros::WallTime start = ros::WallTime::now(); // **** initialize *************************************************** if (!initialized_) { initialized_ = getBaseToCameraTf(rgb_msg->header); init_time_ = rgb_msg->header.stamp; if (!initialized_) return; motion_estimation_.setBaseToCameraTf(eigenAffineFromTf(b2c_)); } // **** create frame ************************************************* ros::WallTime start_frame = ros::WallTime::now(); rgbdtools::RGBDFrame frame; createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame); ros::WallTime end_frame = ros::WallTime::now(); // **** find features ************************************************ ros::WallTime start_features = ros::WallTime::now(); feature_detector_->findFeatures(frame); ros::WallTime end_features = ros::WallTime::now(); // **** registration ************************************************* ros::WallTime start_reg = ros::WallTime::now(); AffineTransform m = motion_estimation_.getMotionEstimation(frame); tf::Transform motion = tfFromEigenAffine(m); f2b_ = motion * f2b_; ros::WallTime end_reg = ros::WallTime::now(); // **** publish outputs ********************************************** if (publish_tf_) publishTf(rgb_msg->header); if (publish_odom_) publishOdom(rgb_msg->header); if (publish_path_) publishPath(rgb_msg->header); if (publish_pose_) publishPoseStamped(rgb_msg->header); if (publish_feature_cloud_) publishFeatureCloud(frame); if (publish_feature_cov_) publishFeatureCovariances(frame); if (publish_model_cloud_) publishModelCloud(); if (publish_model_cov_) publishModelCovariances(); // **** print diagnostics ******************************************* ros::WallTime end = ros::WallTime::now(); frame_count_++; int n_features = frame.keypoints.size(); int n_valid_features = frame.n_valid_keypoints; int n_model_pts = motion_estimation_.getModelSize(); double d_frame = 1000.0 * (end_frame - start_frame ).toSec(); double d_features = 1000.0 * (end_features - start_features).toSec(); double d_reg = 1000.0 * (end_reg - start_reg ).toSec(); double d_total = 1000.0 * (end - start ).toSec(); diagnostics(n_features, n_valid_features, n_model_pts, d_frame, d_features, d_reg, d_total); }