void visit_lqp(const std::shared_ptr<AbstractLQPNode>& lqp, Visitor visitor) {
std::queue<std::shared_ptr<AbstractLQPNode>> node_queue;
node_queue.push(lqp);
std::unordered_set<std::shared_ptr<AbstractLQPNode>> visited_nodes;
while (!node_queue.empty()) {
auto node = node_queue.front();
node_queue.pop();
if (!visited_nodes.emplace(node).second) continue;
if (visitor(node) == LQPVisitation::VisitInputs) {
if (node->left_input()) node_queue.push(node->left_input());
if (node->right_input()) node_queue.push(node->right_input());
}
}
}
bool FootstepPlanner::projectFootPrint(
const Eigen::Affine3f& center_pose,
const Eigen::Affine3f& left_pose_trans,
const Eigen::Affine3f& right_pose_trans,
geometry_msgs::Pose& pose)
{
const Eigen::Vector3f resolution(resolution_x_,
resolution_y_,
resolution_theta_);
const Eigen::Vector3f footstep_size(footstep_size_x_,
footstep_size_y_,
0.000001);
Eigen::Affine3f left_pose = center_pose * left_pose_trans;
Eigen::Affine3f right_pose = center_pose * right_pose_trans;
FootstepState::Ptr left_input(new FootstepState(
jsk_footstep_msgs::Footstep::LEFT,
left_pose,
footstep_size,
resolution));
FootstepState::Ptr right_input(new FootstepState(
jsk_footstep_msgs::Footstep::RIGHT,
right_pose,
footstep_size,
resolution));
FootstepState::Ptr projected_left = graph_->projectFootstep(left_input);
FootstepState::Ptr projected_right = graph_->projectFootstep(right_input);
if (!projected_left || !projected_right) {
return false;
}
Eigen::Affine3f projected_left_pose = projected_left->getPose();
Eigen::Affine3f projected_right_pose = projected_right->getPose();
Eigen::Quaternionf rot = Eigen::Quaternionf(projected_left_pose.rotation()).slerp(
0.5, Eigen::Quaternionf(projected_right_pose.rotation()));
Eigen::Vector3f pos = (Eigen::Vector3f(projected_right_pose.translation()) +
Eigen::Vector3f(projected_left_pose.translation())) / 2.0;
Eigen::Affine3f mid = Eigen::Translation3f(pos) * rot;
tf::poseEigenToMsg(mid, pose);
return true;
}
