FootstepPlanner::FootstepPlanner(ros::NodeHandle& nh):
as_(nh, nh.getNamespace(),
boost::bind(&FootstepPlanner::planCB, this, _1), false)
{
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
typename dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
srv_->setCallback (f);
pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
"text", 1, true);
pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
"close_list", 1, true);
pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
"open_list", 1, true);
srv_project_footprint_ = nh.advertiseService(
"project_footprint", &FootstepPlanner::projectFootPrintService, this);
srv_project_footprint_with_local_search_ = nh.advertiseService(
"project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
{
boost::mutex::scoped_lock lock(mutex_);
if (!readSuccessors(nh)) {
return;
}
JSK_ROS_INFO("building graph");
buildGraph();
JSK_ROS_INFO("build graph done");
}
sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
as_.start();
}
void cameraCB(const sensor_msgs::ImageConstPtr &img,
const sensor_msgs::CameraInfoConstPtr &info) {
//IplImage *ipl_ = new IplImage();
//ipl_ = cvCreateImage(cvSize(img->width, img->height), IPL_DEPTH_8U, 1);
//sensor_msgs::CvBridge bridge;
cv_bridge::CvImagePtr cv_ptr;
//cvResize(bridge.imgMsgToCv(img, "mono8"), ipl_);
//IplImage *ipl_ = bridge.imgMsgToCv(img, "mono8");
//cv_ptr = cv_bridge::toCvCopy(img, "mono8");
cv_ptr = cv_bridge::toCvCopy(img, "mono8");
bool prevImg_update_required = false;
if((flow.cols != (int)img->width) ||
(flow.rows != (int)img->height)) {
JSK_ROS_INFO("make flow");
cv_ptr->image.copyTo(flow);
prevImg_update_required = true;
}
if(prevImg_update_required) {
cv_ptr->image.copyTo(prevImg);
prevImg_update_required = false;
JSK_ROS_INFO("return");
return;
}
//
//JSK_ROS_INFO("subscribe image");
//prevImg.
//cv::Mat *nextImg = new cv::Mat(ipl_);
cv::Mat nextImg(img->height, img->width, CV_8UC1);
//memcpy(nextImg->data, ipl_->imageData, img->height*img->width);
cv_ptr->image.copyTo(nextImg);
cv::calcOpticalFlowFarneback(prevImg, nextImg, flow,
0.5, 3, 15, 3, 5, 1.2, 0 );
// 0.5, 2,
// 16, 4,
// 5, 1.1, 0);
//cv::OPTFLOW_USE_INITIAL_FLOW);
nextImg.copyTo(prevImg);
sensor_msgs::Image result;
result.header = img->header;
result.width = flow.cols;
result.height = flow.rows;
result.encoding = "mono8";
result.step = flow.cols;
result.data.resize(flow.cols * flow.rows);
CvPoint2D32f *ptr = (CvPoint2D32f *)flow.data;
for(int i = 0; i<result.data.size(); i++) {
// copy flow -> result
//result.data[i] = ;
int val = 10 * sqrt(ptr[i].x * ptr[i].x + ptr[i].y * ptr[i].y);
result.data[i] = 255>val?val:255;
}
result_pub_.publish(result);
}
TEST(LogUtils, testJSKROSXXX){
JSK_ROS_DEBUG("Testing JSK_ROS_DEBUG: %ld", ros::Time::now().toNSec());
JSK_ROS_INFO("Testing JSK_ROS_INFO: %ld", ros::Time::now().toNSec());
JSK_ROS_WARN("Testing JSK_ROS_WARN: %ld", ros::Time::now().toNSec());
JSK_ROS_ERROR("Testing JSK_ROS_ERROR: %ld", ros::Time::now().toNSec());
JSK_ROS_FATAL("Testing JSK_ROS_FATAL: %ld", ros::Time::now().toNSec());
JSK_ROS_DEBUG_STREAM("Testing " << "JSK_ROS_DEBUG_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_INFO_STREAM("Testing " << "JSK_ROS_INFO_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_WARN_STREAM("Testing " << "JSK_ROS_WARN_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_ERROR_STREAM("Testing " << "JSK_ROS_ERROR_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_FATAL_STREAM("Testing " << "JSK_ROS_FATAL_STREAM: " << ros::Time::now().toNSec());
JSK_ROS_DEBUG_THROTTLE(1, "Testing JSK_ROS_DEBUG_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_INFO_THROTTLE(1, "Testing JSK_ROS_INFO_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_WARN_THROTTLE(1, "Testing JSK_ROS_WARN_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_ERROR_THROTTLE(1, "Testing JSK_ROS_ERROR_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_FATAL_THROTTLE(1, "Testing JSK_ROS_FATAL_THROTTLE: %ld", ros::Time::now().toNSec());
JSK_ROS_DEBUG_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_DEBUG_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_INFO_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_INFO_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_WARN_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_WARN_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_ERROR_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_ERROR_STREAM_THROTTLE: " << ros::Time::now().toNSec());
JSK_ROS_FATAL_STREAM_THROTTLE(1, "Testing " << "JSK_ROS_FATAL_STREAM_THROTTLE: " << ros::Time::now().toNSec());
}
void FootstepPlanner::profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
{
JSK_ROS_INFO("open list: %lu", solver.getOpenList().size());
JSK_ROS_INFO("close list: %lu", solver.getCloseList().size());
publishText(pub_text_,
(boost::format("open_list: %lu\nclose list:%lu")
% (solver.getOpenList().size()) % (solver.getCloseList().size())).str(),
OK);
if (rich_profiling_) {
pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
solver.openListToPointCloud(open_list_cloud);
solver.closeListToPointCloud(close_list_cloud);
publishPointCloud(close_list_cloud, pub_close_list_, latest_header_);
publishPointCloud(open_list_cloud, pub_open_list_, latest_header_);
}
}
void jsk_pcl_ros::DepthImageCreator::onInit () {
JSK_NODELET_INFO("[%s::onInit]", getName().c_str());
ConnectionBasedNodelet::onInit();
tf_listener_ = TfListenerSingleton::getInstance();
// scale_depth
pnh_->param("scale_depth", scale_depth, 1.0);
JSK_ROS_INFO("scale_depth : %f", scale_depth);
// use fixed transform
pnh_->param("use_fixed_transform", use_fixed_transform, false);
JSK_ROS_INFO("use_fixed_transform : %d", use_fixed_transform);
pnh_->param("use_service", use_service, false);
JSK_ROS_INFO("use_service : %d", use_service);
pnh_->param("use_asynchronous", use_asynchronous, false);
JSK_ROS_INFO("use_asynchronous : %d", use_asynchronous);
pnh_->param("use_approximate", use_approximate, false);
JSK_ROS_INFO("use_approximate : %d", use_approximate);
pnh_->param("info_throttle", info_throttle_, 0);
info_counter_ = 0;
pnh_->param("max_queue_size", max_queue_size_, 3);
// set transformation
std::vector<double> trans_pos(3, 0);
std::vector<double> trans_quat(4, 0); trans_quat[3] = 1.0;
if (pnh_->hasParam("translation")) {
jsk_topic_tools::readVectorParameter(*pnh_, "translation", trans_pos);
}
if (pnh_->hasParam("rotation")) {
jsk_topic_tools::readVectorParameter(*pnh_, "rotation", trans_quat);
}
tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
fixed_transform.setOrigin(btp);
fixed_transform.setRotation(btq);
pub_image_ = advertise<sensor_msgs::Image> (*pnh_, "output", max_queue_size_);
pub_cloud_ = advertise<PointCloud>(*pnh_, "output_cloud", max_queue_size_);
pub_disp_image_ = advertise<stereo_msgs::DisparityImage> (*pnh_, "output_disp", max_queue_size_);
if (use_service) {
service_ = pnh_->advertiseService("set_point_cloud",
&DepthImageCreator::service_cb, this);
}
onInitPostProcess();
}
void FootstepPlanner::pointcloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
JSK_ROS_INFO("pointcloud model is updated");
pointcloud_model_.reset(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*msg, *pointcloud_model_);
if (graph_ && use_pointcloud_model_) {
graph_->setPointCloudModel(pointcloud_model_);
}
}
void FootstepMarker::changePlannerHeuristic(const std::string& heuristic)
{
jsk_interactive_marker::SetHeuristic heuristic_req;
heuristic_req.request.heuristic = heuristic;
if (!ros::service::call("/footstep_planner/set_heuristic", heuristic_req)) {
JSK_ROS_ERROR("failed to set heuristic");
}
else {
JSK_ROS_INFO("Success to set heuristic: %s", heuristic.c_str());
}
}
void processFeedback(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
boost::mutex::scoped_lock(mutex);
// control_name is "sec nsec index"
if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN) {
std::string control_name = feedback->control_name;
JSK_ROS_INFO("control_name: %s", control_name.c_str());
std::list<std::string> splitted_string;
boost::split(splitted_string, control_name, boost::is_space());
jsk_recognition_msgs::Int32Stamped index;
index.header.stamp.sec = boost::lexical_cast<int>(splitted_string.front());
splitted_string.pop_front();
index.header.stamp.nsec = boost::lexical_cast<int>(splitted_string.front());
splitted_string.pop_front();
index.data = boost::lexical_cast<int>(splitted_string.front());
publishClickedPolygon(index);
}
}
/**
format is
successors:
- x: 0
y: 0
theta: 0
- x: 0
y: 0
theta: 0
...
*/
bool FootstepPlanner::readSuccessors(ros::NodeHandle& nh)
{
successors_.clear();
if (!nh.hasParam("successors")) {
JSK_ROS_FATAL("no successors are specified");
return false;
}
XmlRpc::XmlRpcValue successors_xml;
nh.param("successors", successors_xml, successors_xml);
if (successors_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
JSK_ROS_FATAL("successors should be an array");
return false;
}
for (size_t i_successors = 0; i_successors < successors_xml.size(); i_successors++) {
XmlRpc::XmlRpcValue successor_xml;
successor_xml = successors_xml[i_successors];
if (successor_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
JSK_ROS_FATAL("element of successors should be an dictionary");
return false;
}
double x = 0;
double y = 0;
double theta = 0;
if (successor_xml.hasMember("x")) {
x = jsk_topic_tools::getXMLDoubleValue(successor_xml["x"]);
}
if (successor_xml.hasMember("y")) {
y = jsk_topic_tools::getXMLDoubleValue(successor_xml["y"]);
}
if (successor_xml.hasMember("theta")) {
theta = jsk_topic_tools::getXMLDoubleValue(successor_xml["theta"]);
}
Eigen::Affine3f successor = affineFromXYYaw(x, y, theta);
successors_.push_back(successor);
}
JSK_ROS_INFO("%lu successors are defined", successors_.size());
return true;
}
void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
JSK_ROS_DEBUG("DepthImageCreator::publish_points");
if (!pcloud2) return;
bool proc_cloud = true, proc_image = true, proc_disp = true;
if ( pub_cloud_.getNumSubscribers()==0 ) {
proc_cloud = false;
}
if ( pub_image_.getNumSubscribers()==0 ) {
proc_image = false;
}
if ( pub_disp_image_.getNumSubscribers()==0 ) {
proc_disp = false;
}
if( !proc_cloud && !proc_image && !proc_disp) return;
int width = info->width;
int height = info->height;
float fx = info->P[0];
float cx = info->P[2];
float tx = info->P[3];
float fy = info->P[5];
float cy = info->P[6];
Eigen::Affine3f sensorPose;
{
tf::StampedTransform transform;
if(use_fixed_transform) {
transform = fixed_transform;
} else {
try {
tf_listener_->waitForTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp,
ros::Duration(0.001));
tf_listener_->lookupTransform(pcloud2->header.frame_id,
info->header.frame_id,
info->header.stamp, transform);
}
catch ( std::runtime_error e ) {
JSK_ROS_ERROR("%s",e.what());
return;
}
}
tf::Vector3 p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
sensorPose = sensorPose * rot;
if (tx != 0.0) {
Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
sensorPose = sensorPose * trans;
}
#if 0 // debug print
JSK_ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
#endif
}
PointCloud pointCloud;
pcl::RangeImagePlanar rangeImageP;
{
// code here is dirty, some bag is in RangeImagePlanar
PointCloud tpc;
pcl::fromROSMsg(*pcloud2, tpc);
Eigen::Affine3f inv;
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
inv = sensorPose.inverse();
pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
#else
pcl::getInverse(sensorPose, inv);
pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
#endif
Eigen::Affine3f dummytrans;
dummytrans.setIdentity();
rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
width/scale_depth, height/scale_depth,
cx/scale_depth, cy/scale_depth,
fx/scale_depth, fy/scale_depth,
dummytrans); //sensorPose);
}
cv::Mat mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
float *tmpf = (float *)mat.ptr();
for(unsigned int i = 0; i < rangeImageP.height*rangeImageP.width; i++) {
tmpf[i] = rangeImageP.points[i].z;
}
if(scale_depth != 1.0) {
cv::Mat tmpmat(info->height, info->width, CV_32FC1);
cv::resize(mat, tmpmat, cv::Size(info->width, info->height)); // LINEAR
//cv::resize(mat, tmpmat, cv::Size(info->width, info->height), 0.0, 0.0, cv::INTER_NEAREST);
mat = tmpmat;
}
if (proc_image) {
sensor_msgs::Image pubimg;
pubimg.header = info->header;
pubimg.width = info->width;
pubimg.height = info->height;
pubimg.encoding = "32FC1";
pubimg.step = sizeof(float)*info->width;
pubimg.data.resize(sizeof(float)*info->width*info->height);
// publish image
memcpy(&(pubimg.data[0]), mat.ptr(), sizeof(float)*info->height*info->width);
pub_image_.publish(boost::make_shared<sensor_msgs::Image>(pubimg));
}
if(proc_cloud || proc_disp) {
// publish point cloud
pcl::RangeImagePlanar rangeImagePP;
rangeImagePP.setDepthImage ((float *)mat.ptr(),
width, height,
cx, cy, fx, fy);
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
rangeImagePP.header = pcl_conversions::toPCL(info->header);
#else
rangeImagePP.header = info->header;
#endif
if(proc_cloud) {
pub_cloud_.publish(boost::make_shared<pcl::PointCloud<pcl::PointWithRange > >
( (pcl::PointCloud<pcl::PointWithRange>)rangeImagePP) );
}
if(proc_disp) {
stereo_msgs::DisparityImage disp;
#if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
#else
disp.header = rangeImagePP.header;
#endif
disp.image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
disp.image.height = rangeImagePP.height;
disp.image.width = rangeImagePP.width;
disp.image.step = disp.image.width * sizeof(float);
disp.f = fx; disp.T = 0.075;
disp.min_disparity = 0;
disp.max_disparity = disp.T * disp.f / 0.3;
disp.delta_d = 0.125;
disp.image.data.resize (disp.image.height * disp.image.step);
float *data = reinterpret_cast<float*> (&disp.image.data[0]);
float normalization_factor = disp.f * disp.T;
for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
data[y*disp.image.width+x] = normalization_factor / p.z;
}
}
pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
}
}
}
void FootstepPlanner::planCB(
const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
{
boost::mutex::scoped_lock lock(mutex_);
latest_header_ = goal->goal_footstep.header;
JSK_ROS_INFO("planCB");
// check message sanity
if (goal->initial_footstep.footsteps.size() == 0) {
JSK_ROS_ERROR("no initial footstep is specified");
as_.setPreempted();
return;
}
if (goal->goal_footstep.footsteps.size() != 2) {
JSK_ROS_ERROR("Need to specify 2 goal footsteps");
as_.setPreempted();
return;
}
if (use_pointcloud_model_ && !pointcloud_model_) {
JSK_ROS_ERROR("No pointcloud model is yet available");
as_.setPreempted();
return;
}
//ros::WallDuration timeout(goal->timeout.expectedCycleTime().toSec());
ros::WallDuration timeout(10.0);
Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
////////////////////////////////////////////////////////////////////
// set start state
// 0 is always start
Eigen::Affine3f start_pose;
tf::poseMsgToEigen(goal->initial_footstep.footsteps[0].pose, start_pose);
FootstepState::Ptr start(FootstepState::fromROSMsg(
goal->initial_footstep.footsteps[0],
footstep_size,
Eigen::Vector3f(resolution_x_,
resolution_y_,
resolution_theta_)));
graph_->setStartState(start);
if (project_start_state_) {
if (!graph_->projectStart()) {
JSK_ROS_ERROR("Failed to project start state");
publishText(pub_text_,
"Failed to project start",
ERROR);
as_.setPreempted();
return;
}
}
////////////////////////////////////////////////////////////////////
// set goal state
jsk_footstep_msgs::Footstep left_goal, right_goal;
for (size_t i = 0; i < goal->goal_footstep.footsteps.size(); i++) {
FootstepState::Ptr goal_state(FootstepState::fromROSMsg(
goal->goal_footstep.footsteps[i],
footstep_size,
Eigen::Vector3f(resolution_x_,
resolution_y_,
resolution_theta_)));
if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
graph_->setLeftGoalState(goal_state);
left_goal = goal->goal_footstep.footsteps[i];
}
else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
graph_->setRightGoalState(goal_state);
right_goal = goal->goal_footstep.footsteps[i];
}
else {
JSK_ROS_ERROR("unknown goal leg");
as_.setPreempted();
return;
}
}
if (project_goal_state_) {
if (!graph_->projectGoal()) {
JSK_ROS_ERROR("Failed to project goal");
as_.setPreempted();
publishText(pub_text_,
"Failed to project goal",
ERROR);
return;
}
}
// set parameters
if (use_transition_limit_) {
graph_->setTransitionLimit(
TransitionLimitXYZRPY::Ptr(new TransitionLimitXYZRPY(
transition_limit_x_,
transition_limit_y_,
transition_limit_z_,
transition_limit_roll_,
transition_limit_pitch_,
transition_limit_yaw_)));
}
else {
graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
}
graph_->setLocalXMovement(local_move_x_);
graph_->setLocalYMovement(local_move_y_);
graph_->setLocalThetaMovement(local_move_theta_);
graph_->setLocalXMovementNum(local_move_x_num_);
graph_->setLocalYMovementNum(local_move_y_num_);
graph_->setLocalThetaMovementNum(local_move_theta_num_);
graph_->setPlaneEstimationMaxIterations(plane_estimation_max_iterations_);
graph_->setPlaneEstimationMinInliers(plane_estimation_min_inliers_);
graph_->setPlaneEstimationOutlierThreshold(plane_estimation_outlier_threshold_);
graph_->setSupportCheckXSampling(support_check_x_sampling_);
graph_->setSupportCheckYSampling(support_check_y_sampling_);
graph_->setSupportCheckVertexNeighborThreshold(support_check_vertex_neighbor_threshold_);
// Solver setup
FootstepAStarSolver<FootstepGraph> solver(graph_,
close_list_x_num_,
close_list_y_num_,
close_list_theta_num_,
profile_period_,
cost_weight_,
heuristic_weight_);
if (heuristic_ == "step_cost") {
solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
}
else if (heuristic_ == "zero") {
solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
}
else if (heuristic_ == "straight") {
solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
}
else if (heuristic_ == "straight_rotation") {
solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
}
else {
JSK_ROS_ERROR("Unknown heuristics");
as_.setPreempted();
return;
}
solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
ros::WallTime start_time = ros::WallTime::now();
std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
ros::WallTime end_time = ros::WallTime::now();
double planning_duration = (end_time - start_time).toSec();
JSK_ROS_INFO_STREAM("took " << planning_duration << " sec");
JSK_ROS_INFO_STREAM("path: " << path.size());
if (path.size() == 0) {
JSK_ROS_ERROR("Failed to plan path");
publishText(pub_text_,
"Failed to plan",
ERROR);
as_.setPreempted();
return;
}
// Convert path to FootstepArray
jsk_footstep_msgs::FootstepArray ros_path;
ros_path.header = goal->goal_footstep.header;
for (size_t i = 0; i < path.size(); i++) {
ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
}
// finalize path
if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
ros_path.footsteps.push_back(right_goal);
ros_path.footsteps.push_back(left_goal);
}
else if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
ros_path.footsteps.push_back(left_goal);
ros_path.footsteps.push_back(right_goal);
}
result_.result = ros_path;
as_.setSucceeded(result_);
pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
solver.openListToPointCloud(open_list_cloud);
solver.closeListToPointCloud(close_list_cloud);
publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
publishText(pub_text_,
(boost::format("Planning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
% planning_duration % path.size()
% open_list_cloud.points.size()
% close_list_cloud.points.size()).str(),
OK);
}
FootstepMarker::FootstepMarker():
ac_("footstep_planner", true), ac_exec_("footstep_controller", true),
plan_run_(false), lleg_first_(true) {
// read parameters
tf_listener_.reset(new tf::TransformListener);
ros::NodeHandle pnh("~");
ros::NodeHandle nh;
srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh);
typename dynamic_reconfigure::Server<Config>::CallbackType f =
boost::bind (&FootstepMarker::configCallback, this, _1, _2);
srv_->setCallback (f);
pnh.param("foot_size_x", foot_size_x_, 0.247);
pnh.param("foot_size_y", foot_size_y_, 0.135);
pnh.param("foot_size_z", foot_size_z_, 0.01);
pnh.param("lfoot_frame_id", lfoot_frame_id_, std::string("lfsensor"));
pnh.param("rfoot_frame_id", rfoot_frame_id_, std::string("rfsensor"));
pnh.param("show_6dof_control", show_6dof_control_, true);
// pnh.param("use_projection_service", use_projection_service_, false);
// pnh.param("use_projection_topic", use_projection_topic_, false);
pnh.param("always_planning", always_planning_, true);
// if (use_projection_topic_) {
project_footprint_pub_ = pnh.advertise<jsk_interactive_marker::SnapFootPrintInput>("project_footprint", 1);
// }
// read lfoot_offset
readPoseParam(pnh, "lfoot_offset", lleg_offset_);
readPoseParam(pnh, "rfoot_offset", rleg_offset_);
pnh.param("footstep_margin", footstep_margin_, 0.2);
pnh.param("use_footstep_planner", use_footstep_planner_, true);
pnh.param("use_footstep_controller", use_footstep_controller_, true);
pnh.param("use_initial_footstep_tf", use_initial_footstep_tf_, true);
pnh.param("wait_snapit_server", wait_snapit_server_, false);
bool nowait = true;
pnh.param("no_wait", nowait, true);
pnh.param("frame_id", marker_frame_id_, std::string("/map"));
footstep_pub_ = nh.advertise<jsk_footstep_msgs::FootstepArray>("footstep_from_marker", 1);
snapit_client_ = nh.serviceClient<jsk_pcl_ros::CallSnapIt>("snapit");
snapped_pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("snapped_pose", 1);
current_pose_pub_ = pnh.advertise<geometry_msgs::PoseStamped>("current_pose", 1);
estimate_occlusion_client_ = nh.serviceClient<std_srvs::Empty>("require_estimation");
if (!nowait && wait_snapit_server_) {
snapit_client_.waitForExistence();
}
if (pnh.getParam("initial_reference_frame", initial_reference_frame_)) {
use_initial_reference_ = true;
JSK_ROS_INFO_STREAM("initial_reference_frame: " << initial_reference_frame_);
}
else {
use_initial_reference_ = false;
JSK_ROS_INFO("initial_reference_frame is not specified ");
}
server_.reset( new interactive_markers::InteractiveMarkerServer(ros::this_node::getName()));
// menu_handler_.insert( "Snap Legs",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
// menu_handler_.insert( "Reset Legs",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Look Ground",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Execute the Plan",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Force to replan",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
// menu_handler_.insert( "Estimate occlusion",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Cancel Walk",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert( "Toggle 6dof marker",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
// menu_handler_.insert( "Resume Footstep",
// boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("Straight Heuristic",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("Stepcost Heuristic**",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("LLeg First",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
menu_handler_.insert("RLeg First",
boost::bind(&FootstepMarker::menuFeedbackCB, this, _1));
marker_pose_.header.frame_id = marker_frame_id_;
marker_pose_.header.stamp = ros::Time::now();
marker_pose_.pose.orientation.w = 1.0;
resetLegPoses();
// initialize lleg_initial_pose, rleg_initial_pose
lleg_initial_pose_.position.y = footstep_margin_ / 2.0;
lleg_initial_pose_.orientation.w = 1.0;
rleg_initial_pose_.position.y = - footstep_margin_ / 2.0;
rleg_initial_pose_.orientation.w = 1.0;
if (use_initial_reference_) {
while (ros::ok()) {
try {
if (!tf_listener_->waitForTransform(marker_frame_id_, initial_reference_frame_,
ros::Time(0.0), ros::Duration(10.0))) {
ROS_INFO_THROTTLE(1.0,
"waiting for transform %s => %s", marker_frame_id_.c_str(),
initial_reference_frame_.c_str());
continue;
}
ROS_INFO("resolved transform %s => %s", marker_frame_id_.c_str(),
initial_reference_frame_.c_str());
tf::StampedTransform transform;
tf_listener_->lookupTransform(marker_frame_id_, initial_reference_frame_,
ros::Time(0), transform);
marker_pose_.pose.position.x = transform.getOrigin().x();
marker_pose_.pose.position.y = transform.getOrigin().y();
marker_pose_.pose.position.z = transform.getOrigin().z();
marker_pose_.pose.orientation.x = transform.getRotation().x();
marker_pose_.pose.orientation.y = transform.getRotation().y();
marker_pose_.pose.orientation.z = transform.getRotation().z();
marker_pose_.pose.orientation.w = transform.getRotation().w();
break;
}
catch (tf2::TransformException& e) {
ROS_ERROR("Failed to lookup transformation: %s", e.what());
}
}
}
initializeInteractiveMarker();
if (use_footstep_planner_) {
ROS_INFO("waiting planner server...");
ac_.waitForServer();
ROS_INFO("found planner server...");
}
if (use_footstep_controller_) {
ROS_INFO("waiting controller server...");
ac_exec_.waitForServer();
ROS_INFO("found controller server...");
}
move_marker_sub_ = nh.subscribe("move_marker", 1, &FootstepMarker::moveMarkerCB, this);
menu_command_sub_ = nh.subscribe("menu_command", 1, &FootstepMarker::menuCommandCB, this);
exec_sub_ = pnh.subscribe("execute", 1, &FootstepMarker::executeCB, this);
resume_sub_ = pnh.subscribe("resume", 1, &FootstepMarker::resumeCB, this);
plan_if_possible_srv_ = pnh.advertiseService("force_to_replan", &FootstepMarker::forceToReplan, this);
if (use_initial_footstep_tf_) {
// waiting TF
while (ros::ok()) {
try {
if (tf_listener_->waitForTransform(lfoot_frame_id_, marker_frame_id_,
ros::Time(0.0), ros::Duration(10.0))
&& tf_listener_->waitForTransform(rfoot_frame_id_, marker_frame_id_,
ros::Time(0.0), ros::Duration(10.0))) {
break;
}
ROS_INFO("waiting for transform {%s, %s} => %s", lfoot_frame_id_.c_str(),
rfoot_frame_id_.c_str(), marker_frame_id_.c_str());
}
catch (tf2::TransformException& e) {
ROS_ERROR("Failed to lookup transformation: %s", e.what());
}
}
ROS_INFO("resolved transform {%s, %s} => %s", lfoot_frame_id_.c_str(),
rfoot_frame_id_.c_str(), marker_frame_id_.c_str());
}
// if (use_projection_topic_) {
projection_sub_ = pnh.subscribe("projected_pose", 1,
&FootstepMarker::projectionCallback, this);
// }
}
void HintedHandleEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const geometry_msgs::PointStampedConstPtr &point_msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PassThrough<pcl::PointXYZ> pass;
int K = 1;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
geometry_msgs::PointStamped transed_point;
ros::Time now = ros::Time::now();
try
{
listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
}
catch(tf::TransformException ex)
{
JSK_ROS_ERROR("%s", ex.what());
return;
}
pcl::PointXYZ searchPoint;
searchPoint.x = transed_point.point.x;
searchPoint.y = transed_point.point.y;
searchPoint.z = transed_point.point.z;
//remove too far cloud
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
pass.filter(*cloud);
if(cloud->points.size() < 10){
JSK_ROS_INFO("points are too small");
return;
}
if(1){ //estimate_normal
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(kd_tree);
ne.setRadiusSearch(0.02);
ne.setViewPoint(0, 0, 0);
ne.compute(*cloud_normals);
}
else{ //use normal of msg
}
if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
JSK_ROS_INFO("kdtree failed");
return;
}
float x = cloud->points[pointIdxNKNSearch[0]].x;
float y = cloud->points[pointIdxNKNSearch[0]].y;
float z = cloud->points[pointIdxNKNSearch[0]].z;
float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
double theta = acos(v_x);
// use normal for estimating handle direction
tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
tf::Quaternion final_quaternion = normal;
double min_theta_index = 0;
double min_width = 100;
tf::Quaternion min_qua(0, 0, 0, 1);
visualization_msgs::Marker debug_hand_marker;
debug_hand_marker.header = cloud_msg->header;
debug_hand_marker.ns = string("debug_grasp");
debug_hand_marker.id = 0;
debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
debug_hand_marker.pose.orientation.w = 1;
debug_hand_marker.scale.x=0.003;
tf::Matrix3x3 best_mat;
//search 180 degree and calc the shortest direction
for(double theta_=0; theta_<3.14/2;
theta_+=3.14/2/30){
tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
tf::Quaternion temp_qua = normal * rotate_;
tf::Matrix3x3 temp_mat(temp_qua);
geometry_msgs::Pose pose_respected_to_tf;
pose_respected_to_tf.position.x = x;
pose_respected_to_tf.position.y = y;
pose_respected_to_tf.position.z = z;
pose_respected_to_tf.orientation.x = temp_qua.getX();
pose_respected_to_tf.orientation.y = temp_qua.getY();
pose_respected_to_tf.orientation.z = temp_qua.getZ();
pose_respected_to_tf.orientation.w = temp_qua.getW();
Eigen::Affine3d box_pose_respected_to_cloud_eigend;
tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
= box_pose_respected_to_cloud_eigend.inverse();
Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
= box_pose_respected_to_cloud_eigend_inversed.matrix();
jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
box_pose_respected_to_cloud_eigen_inversed_matrixd,
box_pose_respected_to_cloud_eigen_inversed_matrixf);
Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output_cloud, offset);
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(output_cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
pass.filter(*points_z);
pass.setInputCloud(points_z);
pass.setFilterFieldName("z");
pass.setFilterLimits(-handle.finger_d, handle.finger_d);
pass.filter(*points_yz);
pass.setInputCloud(points_yz);
pass.setFilterFieldName("x");
pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
pass.filter(*points_xyz);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
for(size_t index=0; index<points_xyz->size(); index++){
points_xyz->points[index].x = points_xyz->points[index].z = 0;
}
if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
kdtree.setInputCloud(points_xyz);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZ search_point_tree;
search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
double before_w=10, temp_w;
for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
temp_w =sqrt(pointRadiusSquaredDistance[index]);
if(temp_w - before_w > handle.finger_w*2){
break; // there are small space for finger
}
before_w=temp_w;
}
if(before_w < min_width){
min_theta_index = theta_;
min_width = before_w;
min_qua = temp_qua;
best_mat = temp_mat;
}
//for debug view
geometry_msgs::Point temp_point;
std_msgs::ColorRGBA temp_color;
temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
}
}
geometry_msgs::PoseStamped handle_pose_stamped;
handle_pose_stamped.header = cloud_msg->header;
handle_pose_stamped.pose.position.x = x;
handle_pose_stamped.pose.position.y = y;
handle_pose_stamped.pose.position.z = z;
handle_pose_stamped.pose.orientation.x = min_qua.getX();
handle_pose_stamped.pose.orientation.y = min_qua.getY();
handle_pose_stamped.pose.orientation.z = min_qua.getZ();
handle_pose_stamped.pose.orientation.w = min_qua.getW();
std_msgs::Float64 min_width_msg;
min_width_msg.data = min_width;
pub_pose_.publish(handle_pose_stamped);
pub_debug_marker_.publish(debug_hand_marker);
pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
jsk_recognition_msgs::SimpleHandle simple_handle;
simple_handle.header = handle_pose_stamped.header;
simple_handle.pose = handle_pose_stamped.pose;
simple_handle.handle_width = min_width;
pub_handle_.publish(simple_handle);
}
void ColorizeRandomForest::extract(const sensor_msgs::PointCloud2 pc)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
std::vector<int> indices;
pcl::fromROSMsg(pc, *cloud);
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName (std::string("z"));
pass.setFilterLimits (0.0, 1.5);
pass.filter (*cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.025);
ec.setMinClusterSize (200);
ec.setMaxClusterSize (100000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr noncloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
int cluster_num = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
JSK_ROS_INFO("Calculate time %d / %ld", cluster_num , cluster_indices.size());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]);
cloud_cluster->is_dense = true;
cluster_num ++ ;
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
ne.setInputCloud (cloud_cluster);
ne.setSearchMethod (tree);
ne.setRadiusSearch (0.02);
ne.compute (*cloud_normals);
for (int cloud_index = 0; cloud_index < cloud_normals->points.size(); cloud_index++) {
cloud_normals->points[cloud_index].x = cloud_cluster->points[cloud_index].x;
cloud_normals->points[cloud_index].y = cloud_cluster->points[cloud_index].y;
cloud_normals->points[cloud_index].z = cloud_cluster->points[cloud_index].z;
}
int result_counter=0, call_counter = 0;
pcl::PointXYZRGBNormal max_pt,min_pt;
pcl::getMinMax3D(*cloud_normals, min_pt, max_pt);
for (int i = 0 ; i < 30; i++) {
double lucky = 0, lucky2 = 0;
std::string axis("x"), other_axis("y");
int rand_xy = rand()%2;
if (rand_xy == 0) {
lucky = min_pt.x - pass_offset_ + (max_pt.x - min_pt.x - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
lucky2 = min_pt.y + (max_pt.y - min_pt.y) * 1.0 * rand() / RAND_MAX;
} else {
lucky = min_pt.y - pass_offset_ + (max_pt.y - min_pt.y - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
lucky2 = min_pt.x + (max_pt.x - min_pt.x) * 1.0 * rand() / RAND_MAX;
axis = std::string("y");
other_axis = std::string("x");
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals_pass (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
pcl::IndicesPtr indices_x(new std::vector<int>());
pass.setInputCloud (cloud_normals);
pass.setFilterFieldName (axis);
float small = std::min(lucky, lucky + pass_offset_);
float large = std::max(lucky, lucky + pass_offset_);
pass.setFilterLimits (small, large);
pass.filter (*cloud_normals_pass);
pass.setInputCloud (cloud_normals_pass);
pass.setFilterFieldName (other_axis);
float small2 = std::min(lucky2, lucky2 + pass_offset2_);
float large2 = std::max(lucky2, lucky2 + pass_offset2_);
pass.setFilterLimits (small2, large2);
pass.filter (*cloud_normals_pass);
std::vector<int> tmp_indices;
pcl::removeNaNFromPointCloud(*cloud_normals_pass, *cloud_normals_pass, tmp_indices);
if(cloud_normals_pass->points.size() == 0)
continue;
pcl::FPFHEstimationOMP<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::FPFHSignature33> fpfh;
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());
fpfh.setNumberOfThreads(8);
fpfh.setInputCloud (cloud_normals_pass);
fpfh.setInputNormals (cloud_normals_pass);
fpfh.setSearchMethod (tree);
fpfh.setRadiusSearch (radius_search_);
fpfh.compute (*fpfhs);
if((int)fpfhs->points.size() == 0)
continue;
std::vector<double> result;
int target_id, max_value;
if ((int)fpfhs->points.size() - sum_num_ - 1 < 1) {
target_id = 0;
max_value = (int)fpfhs->points.size();
} else {
target_id = rand() % ((int)fpfhs->points.size() - sum_num_ - 1);
max_value = target_id + sum_num_;
}
bool has_nan = false;
for(int index = 0; index < 33; index++) {
float sum_hist_points = 0;
for(int kndex = target_id; kndex < max_value; kndex++)
{
sum_hist_points+=fpfhs->points[kndex].histogram[index];
}
result.push_back( sum_hist_points/sum_num_ );
}
for(int x = 0; x < result.size(); x ++) {
if(pcl_isnan(result[x]))
has_nan = true;
}
if(has_nan)
break;
call_counter++;
ros::ServiceClient client = pnh_->serviceClient<ml_classifiers::ClassifyData>("/predict");
ml_classifiers::ClassifyData srv;
ml_classifiers::ClassDataPoint class_data_point;
class_data_point.point = result;
srv.request.data.push_back(class_data_point);
if(client.call(srv))
if (atoi(srv.response.classifications[0].c_str()) == 0)
result_counter += 1;
JSK_ROS_INFO("response result : %s", srv.response.classifications[0].c_str());
}
if(result_counter >= call_counter / 2) {
JSK_ROS_INFO("Cloth %d / %d", result_counter, call_counter);
}
else {
JSK_ROS_INFO("Not Cloth %d / %d", result_counter, call_counter);
}
for (int i = 0; i < cloud_cluster->points.size(); i++) {
if(result_counter >= call_counter / 2) {
cloth_cloud_cluster->points.push_back (cloud_cluster->points[i]);
}
else {
noncloth_cloud_cluster->points.push_back (cloud_cluster->points[i]);
}
}
}
sensor_msgs::PointCloud2 cloth_pointcloud2;
pcl::toROSMsg(*cloth_cloud_cluster, cloth_pointcloud2);
cloth_pointcloud2.header = pc.header;
cloth_pointcloud2.is_dense = false;
pub_.publish(cloth_pointcloud2);
sensor_msgs::PointCloud2 noncloth_pointcloud2;
pcl::toROSMsg(*noncloth_cloud_cluster, noncloth_pointcloud2);
noncloth_pointcloud2.header = pc.header;
noncloth_pointcloud2.is_dense = false;
pub2_.publish(noncloth_pointcloud2);
}
