void OrganizePointCloud::extract(const sensor_msgs::PointCloud2ConstPtr &input)
{
// skip empty cloud
JSK_ROS_INFO_STREAM("received input clouds, convert range image, resolution: " << angular_resolution << ", width(deg): " << angle_width << ", height(deg):" << angle_height << ", min_points:" << min_points);
if ( input->width < min_points ) return;
pcl::PointCloud<pcl::PointXYZ> pointCloud;
pcl::fromROSMsg(*input, pointCloud);
// We now want to create a range image from the above point cloud, with a 1deg angular resolution
float angularResolution = (float) (angular_resolution * (M_PI/180.0f)); // 1.0 degree in radians
float maxAngleWidth = (float) (angle_width * (M_PI/180.0f)); // 120.0 degree in radians
float maxAngleHeight = (float) (angle_height * (M_PI/180.0f)); // 90.0 degree in radians
Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
float noiseLevel=0.00;
float minRange = 0.0f;
int borderSize = 1;
pcl::RangeImage rangeImage;
rangeImage.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight,
sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
JSK_ROS_INFO_STREAM("input image size " << input->width << " x " << input->height << "(=" << input->width * input->height << ")");
JSK_ROS_INFO_STREAM("output image size " << rangeImage.width << " x " << rangeImage.height << "(=" << rangeImage.width * rangeImage.height << ")");
JSK_ROS_DEBUG_STREAM(rangeImage);
sensor_msgs::PointCloud2 out;
pcl::toROSMsg(rangeImage, out);
out.header = input->header;
pub_.publish(out);
}
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());
}
bool jsk_pcl_ros::PointcloudScreenpoint::checkpoint (pcl::PointCloud< pcl::PointXYZ > &in_pts, int x, int y,
float &resx, float &resy, float &resz) {
if ((x < 0) || (y < 0) || (x >= in_pts.width) || (y >= in_pts.height)) return false;
pcl::PointXYZ p = in_pts.points[in_pts.width * y + x];
// search near points
JSK_ROS_INFO_STREAM("Request: screenpoint ("<<x<<","<<y<<")="<<"(" << p.x << ", " <<p.y << ", " <<p.z <<")");
//return !(isnan (p.x) || ( (p.x == 0.0) && (p.y == 0.0) && (p.z == 0.0)));
if ( !isnan (p.x) && ((p.x != 0.0) || (p.y != 0.0) || (p.z == 0.0)) ) {
resx = p.x; resy = p.y; resz = p.z;
return true;
}
return false;
}
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);
// }
}
