Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[jsk_footstep_planner] add comments to FootstepPlanner::planCB() in footstep_planner.cpp #737

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

[jsk_footstep_planner] add comments to FootstepPlanner::planCB() in footstep_planner.cpp #737

Changes from 3 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
bcc06ac
[jsk_footstep_planner] add comments to FootstepPlanner::planCB
sktometometo Nov 17, 2019
9c6327e
[jsk_footstep_planner] split object initialization from verifying pro…
sktometometo Nov 17, 2019
10b2948
[jsk_footstep_planner] delete unnecessary comments
sktometometo Nov 18, 2019
138f2cd
[jsk_footstep_planner] add comments
sktometometo Mar 22, 2020
7f7f6f7
Merge branch 'master' into feature/add_comment_footstep_planner
k-okada May 11, 2021
e200c78
[jsk_footstep_planner] remove unused sample code
sktometometo Jun 11, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
80 changes: 69 additions & 11 deletions jsk_footstep_planner/src/footstep_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -407,13 +407,33 @@ namespace jsk_footstep_planner
pub.publish(msg);
}

/*
* Callback function of actionlib server to plan footsteps to a given goal
*/
void FootstepPlanner::planCB(
const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
{
boost::mutex::scoped_lock lock(mutex_);

// Store header of latest action goal footstep
latest_header_ = goal->goal_footstep.header;
ROS_INFO("planCB");
// check message sanity


/*
* Verifying whether a requested message meets requirements below or not
* - FootstepArray in the "initial_footstep" record of a goal is set.
* - The size of FootstepArray in the "initial_footstep" record of a goal is 2.
* - The frame_id of the "initial_footstep" record of a goal is equal to
* the frame_id of pointcloud
* - The frame_id of the "initial_footstep" record of a goal is equal to
* the frame_id of obstacle pointcloudif obstacle_mode is used.
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[Trivial] Need space before if

* - The size of FootstepArray in the "goal_footstep" record of a goal is 2.
*
* And verifying if settings below is correct.
* - pointcloud options are correctly set.
*/
// check messgae sanity
if (goal->initial_footstep.footsteps.size() == 0) {
ROS_ERROR("no initial footstep is specified");
as_.setPreempted();
Expand Down Expand Up @@ -449,15 +469,23 @@ namespace jsk_footstep_planner
return;
}
}
Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
Eigen::Vector3f search_resolution(resolution_x_, resolution_y_, resolution_theta_);
// check goal is whether collision free
// conevrt goal footstep into FootstepState::Ptr instance.
if (goal->goal_footstep.footsteps.size() != 2) {
ROS_ERROR("goal footstep should be a pair of footsteps");
as_.setPreempted();
return;
}


/*
* Initialize footstep_size and search_resolution
*/
Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
Eigen::Vector3f search_resolution(resolution_x_, resolution_y_, resolution_theta_);


/*
* Convert goal footstep (Footstep message instance) into FootstepState::Ptr instance.
*/
std::vector<jsk_footstep_msgs::Footstep > goal_ros;
goal_ros.push_back(goal->goal_footstep.footsteps[0]);
goal_ros.push_back(goal->goal_footstep.footsteps[1]);
Expand All @@ -483,18 +511,26 @@ namespace jsk_footstep_planner
goal_ros[i].dimensions.z = 0.000001;
}
}

FootstepState::Ptr first_goal = FootstepState::fromROSMsg(goal_ros[0],
footstep_size,
search_resolution);
FootstepState::Ptr second_goal = FootstepState::fromROSMsg(goal_ros[1],
footstep_size,
search_resolution);

/*
* Check whether a given goal is reachable or not
*/
// check goal is whether collision free
if (!graph_->isSuccessable(second_goal, first_goal)) {
ROS_ERROR("goal is non-realistic");
as_.setPreempted();
return;
}

/*
* Set Timeout
*/
ros::WallDuration timeout;
if(goal->timeout.toSec() == 0.0) {
timeout = ros::WallDuration(planning_timeout_);
Expand All @@ -503,6 +539,9 @@ namespace jsk_footstep_planner
}


/*
* Initialize search tree with start footstep state and goal footstep state
*/
////////////////////////////////////////////////////////////////////
// set start state
// 0 is always start
Expand Down Expand Up @@ -536,7 +575,6 @@ namespace jsk_footstep_planner
return;
}
}

////////////////////////////////////////////////////////////////////
// set goal state
jsk_footstep_msgs::Footstep left_goal, right_goal;
Expand Down Expand Up @@ -571,6 +609,7 @@ namespace jsk_footstep_planner
return;
}
}
////////////////////////////////////////////////////////////////////
// set parameters
if (parameters_.use_transition_limit) {
graph_->setTransitionLimit(
Expand All @@ -594,17 +633,19 @@ namespace jsk_footstep_planner
TransitionLimitRP::Ptr(new TransitionLimitRP(
parameters_.global_transition_limit_roll,
parameters_.global_transition_limit_pitch)));

}
else {
graph_->setGlobalTransitionLimit(TransitionLimitRP::Ptr());
}
graph_->setParameters(parameters_);

graph_->setSuccessorFunction(boost::bind(&FootstepGraph::successors_original, graph_, _1, _2));
graph_->setPathCostFunction(boost::bind(&FootstepGraph::path_cost_original, graph_, _1, _2, _3));

//ROS_INFO_STREAM(graph_->infoString());


/*
* Setting up a solver for footstep graph
*/
// Solver setup
FootstepAStarSolver<FootstepGraph> solver(graph_,
close_list_x_num_,
Expand Down Expand Up @@ -635,13 +676,24 @@ namespace jsk_footstep_planner
}
graph_->clearPerceptionDuration();
solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));


/*
* Plan footsteps using the solver
*/
ROS_INFO("start_solver timeout: %f", timeout.toSec());
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();
//ROS_INFO_STREAM("took " << planning_duration << " sec");
//ROS_INFO_STREAM("path: " << path.size());


/*
* Finalize footstep path
*/
// when solver failed
if (path.size() == 0) {
pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
solver.openListToPointCloud(open_list_cloud);
Expand All @@ -666,7 +718,8 @@ namespace jsk_footstep_planner
as_.setPreempted();
return;
}
// finalize in graph
// when solver succeeded
// finalize in graph (add exact final footsteps to result footstep
std::vector <FootstepState::Ptr> finalizeSteps;
if (! (graph_->finalizeSteps((path.size() >1 ? path[path.size()-2]->getState() : FootstepState::Ptr()),
path[path.size()-1]->getState(),
Expand Down Expand Up @@ -697,9 +750,14 @@ namespace jsk_footstep_planner
ros_path.footsteps.push_back(*(st->toROSMsg(inv_rleg_footstep_offset_)));
}
}
// return result of action server
result_.result = ros_path;
as_.setSucceeded(result_);


/*
* Publish the result of planning
*/
pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
solver.openListToPointCloud(open_list_cloud);
solver.closeListToPointCloud(close_list_cloud);
Expand Down