bool FootstepPlanner::projectFootPrintService( jsk_interactive_marker::SnapFootPrint::Request& req, jsk_interactive_marker::SnapFootPrint::Response& res) { boost::mutex::scoped_lock lock(mutex_); if (!graph_) { return false; } if (!pointcloud_model_) { JSK_ROS_ERROR("No pointcloud model is yet available"); publishText(pub_text_, "No pointcloud model is yet available", ERROR); return false; } Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans; tf::poseMsgToEigen(req.lleg_pose, left_pose_trans); tf::poseMsgToEigen(req.rleg_pose, right_pose_trans); tf::poseMsgToEigen(req.input_pose.pose, center_pose); if (projectFootPrint(center_pose, left_pose_trans, right_pose_trans, res.snapped_pose.pose)) { res.success = true; res.snapped_pose.header = req.input_pose.header; return true; } else { JSK_ROS_ERROR("Failed to project footprint"); publishText(pub_text_, "Failed to project goal", ERROR); return false; } }
bool FootstepPlanner::projectFootPrintWithLocalSearchService( jsk_interactive_marker::SnapFootPrint::Request& req, jsk_interactive_marker::SnapFootPrint::Response& res) { boost::mutex::scoped_lock lock(mutex_); if (!graph_ ) { return false; } if (!pointcloud_model_) { JSK_ROS_ERROR("No pointcloud model is yet available"); publishText(pub_text_, "No pointcloud model is yet available", ERROR); return false; } Eigen::Affine3f center_pose, left_pose_trans, right_pose_trans; std::vector<Eigen::Affine3f> center_poses; tf::poseMsgToEigen(req.lleg_pose, left_pose_trans); tf::poseMsgToEigen(req.rleg_pose, right_pose_trans); tf::poseMsgToEigen(req.input_pose.pose, center_pose); const double dx = 0.05; const double dy = 0.05; const double dtheta = pcl::deg2rad(5.0); for (int xi = 0; xi < 3; xi++) { for (int yi = 0; yi < 3; yi++) { for (int thetai = 0; thetai < 3; thetai++) { Eigen::Affine3f transppp = affineFromXYYaw(xi * dx, yi * dy, thetai * dtheta); Eigen::Affine3f transppm = affineFromXYYaw(xi * dx, yi * dy, - thetai * dtheta); Eigen::Affine3f transpmp = affineFromXYYaw(xi * dx, - yi * dy, thetai * dtheta); Eigen::Affine3f transpmm = affineFromXYYaw(xi * dx, - yi * dy, -thetai * dtheta); Eigen::Affine3f transmpp = affineFromXYYaw(- xi * dx, yi * dy, thetai * dtheta); Eigen::Affine3f transmpm = affineFromXYYaw(- xi * dx, yi * dy, - thetai * dtheta); Eigen::Affine3f transmmp = affineFromXYYaw(- xi * dx, - yi * dy, thetai * dtheta); Eigen::Affine3f transmmm = affineFromXYYaw(- xi * dx, - yi * dy, - thetai * dtheta); center_poses.push_back(center_pose * transppp); center_poses.push_back(center_pose * transppm); center_poses.push_back(center_pose * transpmp); center_poses.push_back(center_pose * transpmm); center_poses.push_back(center_pose * transmpp); center_poses.push_back(center_pose * transmpm); center_poses.push_back(center_pose * transmmp); center_poses.push_back(center_pose * transmmm); } } } for (size_t i = 0; i < center_poses.size(); i++) { if (projectFootPrint(center_poses[i], left_pose_trans, right_pose_trans, res.snapped_pose.pose)) { res.success = true; res.snapped_pose.header = req.input_pose.header; return true; } } JSK_ROS_ERROR("Failed to project footprint"); publishText(pub_text_, "Failed to project goal", ERROR); return false; }
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 imageCallback(const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { JSK_ROS_ERROR("cv_bridge exception: %s", e.what()); return; } cv::Mat cv_img = cv_ptr->image; cv::Mat cv_og_img; cv::Mat cv_out_img; // calcOrientedGradient() will write oriented gradient to // 2nd arg image as HSV format, // H is orientation and V is intensity calcOrientedGradient(cv_img, cv_og_img); cv::cvtColor(cv_og_img, cv_out_img, CV_HSV2BGR); // cv::imshow("OrinetedGradient", cv_out_img); // cv::waitKey(1); sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage( msg->header, sensor_msgs::image_encodings::BGR8, cv_out_img).toImageMsg(); image_pub_.publish(out_img); }
bool jsk_pcl_ros::PointcloudScreenpoint::screenpoint_cb (jsk_pcl_ros::TransformScreenpoint::Request &req, jsk_pcl_ros::TransformScreenpoint::Response &res) { JSK_ROS_DEBUG("PointcloudScreenpoint::screenpoint_cb"); boost::mutex::scoped_lock lock(this->mutex_callback_); if ( pts_.points.size() == 0 ) { JSK_ROS_ERROR("no point cloud was received"); return false; } res.header = header_; bool ret; float rx, ry, rz; ret = extract_point (pts_, req.x, req.y, rx, ry, rz); res.point.x = rx; res.point.y = ry; res.point.z = rz; if (!ret) { return false; } // search normal n3d_.setSearchSurface(boost::make_shared<pcl::PointCloud<pcl::PointXYZ > > (pts_)); pcl::PointCloud<pcl::PointXYZ> cloud_; pcl::PointXYZ pt; pt.x = res.point.x; pt.y = res.point.y; pt.z = res.point.z; cloud_.points.resize(0); cloud_.points.push_back(pt); n3d_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ > > (cloud_)); pcl::PointCloud<pcl::Normal> cloud_normals; n3d_.compute (cloud_normals); res.vector.x = cloud_normals.points[0].normal_x; res.vector.y = cloud_normals.points[0].normal_y; res.vector.z = cloud_normals.points[0].normal_z; if((res.point.x * res.vector.x + res.point.y * res.vector.y + res.point.z * res.vector.z) < 0) { res.vector.x *= -1; res.vector.y *= -1; res.vector.z *= -1; } if (publish_point_) { geometry_msgs::PointStamped ps; ps.header = header_; ps.point.x = res.point.x; ps.point.y = res.point.y; ps.point.z = res.point.z; pub_point_.publish(ps); } return true; }
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 PolygonArrayDistanceLikelihood::onInit() { DiagnosticNodelet::onInit(); if (!pnh_->getParam("target_frame_id", target_frame_id_)) { JSK_ROS_ERROR("You need to specify ~target_frame_id"); return; } pnh_->param("tf_queue_size", tf_queue_size_, 10); tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance(); pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1); }
void OrganizedPointCloudToPointIndices::indices( const sensor_msgs::PointCloud2::ConstPtr& point_msg) { pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*point_msg, *pc); if(pc->isOrganized()){ PCLIndicesMsg indices_msg; indices_msg.header = point_msg->header; for (size_t i = 0; i < pc->points.size(); i++) if (!isnan(pc->points[i].x) && !isnan(pc->points[i].y) && !isnan(pc->points[i].z)) indices_msg.indices.push_back(i); pub_.publish(indices_msg); }else{ JSK_ROS_ERROR("Input Pointcloud is not organized"); } }
void PolygonArrayAngleLikelihood::onInit() { DiagnosticNodelet::onInit(); if (!pnh_->getParam("target_frame_id", target_frame_id_)) { JSK_ROS_ERROR("You need to specify ~target_frame_id"); return; } pnh_->param("tf_queue_size", tf_queue_size_, 10); std::vector<double> axis(3); if (!jsk_topic_tools::readVectorParameter(*pnh_, "axis", axis)) { axis[0] = 1; axis[1] = 0; axis[2] = 0; } axis_[0] = axis[0]; axis_[1] = axis[1]; axis_[2] = axis[2]; tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance(); pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1); }
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 PlaneSupportedCuboidEstimator::estimate( const sensor_msgs::PointCloud2::ConstPtr& msg) { // Update polygons_ vector polygons_.clear(); for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) { Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon); polygon->decomposeToTriangles(); polygons_.push_back(polygon); } try { // viewpoint tf::StampedTransform transform = lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id, ros::Time(0.0), ros::Duration(0.0)); tf::vectorTFToEigen(transform.getOrigin(), viewpoint_); if (!tracker_) { NODELET_INFO("initTracker"); pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr particles = initParticles(); tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>); tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1)); tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2)); tracker_->setParticles(particles); tracker_->setParticleNum(particle_num_); } else { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*msg, *cloud); tracker_->setInputCloud(cloud); // Before call compute() method, we prepare candidate_cloud_ for // weight phase. candidate_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>); std::set<int> all_indices; for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) { Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon); pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism_extract; pcl::PointCloud<pcl::PointXYZ>::Ptr boundaries (new pcl::PointCloud<pcl::PointXYZ>); polygon->boundariesToPointCloud<pcl::PointXYZ>(*boundaries); boundaries->points.push_back(boundaries->points[0]); prism_extract.setInputCloud(cloud); prism_extract.setViewPoint(viewpoint_[0], viewpoint_[1], viewpoint_[2]); prism_extract.setHeightLimits(init_local_position_z_min_, init_local_position_z_max_); prism_extract.setInputPlanarHull(boundaries); pcl::PointIndices output_indices; prism_extract.segment(output_indices); for (size_t i = 0; i < output_indices.indices.size(); i++) { all_indices.insert(output_indices.indices[i]); } } pcl::ExtractIndices<pcl::PointXYZ> ex; ex.setInputCloud(cloud); pcl::PointIndices::Ptr indices (new pcl::PointIndices); indices->indices = std::vector<int>(all_indices.begin(), all_indices.end()); ex.setIndices(indices); ex.filter(*candidate_cloud_); sensor_msgs::PointCloud2 ros_candidate_cloud; pcl::toROSMsg(*candidate_cloud_, ros_candidate_cloud); ros_candidate_cloud.header = msg->header; pub_candidate_cloud_.publish(ros_candidate_cloud); // check the number of candidate points if (candidate_cloud_->points.size() == 0) { JSK_NODELET_ERROR("No candidate cloud"); return; } tree_.setInputCloud(candidate_cloud_); if (support_plane_updated_) { // Compute assignment between particles and polygons NODELET_INFO("polygon updated"); updateParticlePolygonRelationship(tracker_->getParticles()); } ROS_INFO("start tracker_->compute()"); tracker_->compute(); ROS_INFO("done tracker_->compute()"); Particle result = tracker_->getResult(); jsk_recognition_msgs::BoundingBoxArray box_array; box_array.header = msg->header; box_array.boxes.push_back(result.toBoundingBox()); box_array.boxes[0].header = msg->header; pub_result_.publish(box_array); } support_plane_updated_ = false; ParticleCloud::Ptr particles = tracker_->getParticles(); // Publish histograms publishHistogram(particles, 0, pub_histogram_global_x_, msg->header); publishHistogram(particles, 1, pub_histogram_global_y_, msg->header); publishHistogram(particles, 2, pub_histogram_global_z_, msg->header); publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header); publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header); publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header); publishHistogram(particles, 6, pub_histogram_dx_, msg->header); publishHistogram(particles, 7, pub_histogram_dy_, msg->header); publishHistogram(particles, 8, pub_histogram_dz_, msg->header); // Publish particles sensor_msgs::PointCloud2 ros_particles; pcl::toROSMsg(*particles, ros_particles); ros_particles.header = msg->header; pub_particles_.publish(ros_particles); } catch (tf2::TransformException& e) { JSK_ROS_ERROR("tf exception"); } }
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); }
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); }