FootstepState::Ptr FootstepGraph::projectFootstep(FootstepState::Ptr in,
                                                   unsigned int& error_state)
 {
   if(!use_pointcloud_model_) {
     return in;
   }
   ros::WallTime start_time = ros::WallTime::now();
   FootstepState::Ptr projected_footstep = in->projectToCloud(
     *tree_model_,
     pointcloud_model_,
     grid_search_,
     *tree_model_2d_,
     pointcloud_model_2d_,
     Eigen::Vector3f(0, 0, 1),
     error_state, parameters_);
   ros::WallTime end_time = ros::WallTime::now();
   perception_duration_ = perception_duration_ + (end_time  - start_time);
   return projected_footstep;
 }
Example #2
0
 FootstepState::Ptr FootstepGraph::projectFootstep(FootstepState::Ptr in,
                                                   unsigned int& error_state)
 {
   ros::WallTime start_time = ros::WallTime::now();
   FootstepState::Ptr projected_footstep = in->projectToCloud(
     *tree_model_,
     pointcloud_model_,
     grid_search_,
     *tree_model_2d_,
     pointcloud_model_2d_,
     Eigen::Vector3f(0, 0, 1),
     error_state,
     plane_estimation_outlier_threshold_,
     plane_estimation_max_iterations_,
     plane_estimation_min_inliers_,
     support_check_x_sampling_,
     support_check_y_sampling_,
     support_check_vertex_neighbor_threshold_);
   ros::WallTime end_time = ros::WallTime::now();
   perception_duration_ = perception_duration_ + (end_time  - start_time);
   return projected_footstep;
 }