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;
}
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_);
}
}
/* Receving navdata during the event loop */
inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
{
const navdata_demo_t*nd = &navdata->navdata_demo;
printf("============================= Publishing data ===============================");
publishText();
printf("=====================\nNavdata for flight demonstrations =====================\n\n");
printf("Control state : %i\n",nd->ctrl_state);
printf("Battery level : %i mV\n",nd->vbat_flying_percentage);
printf("Orientation : [Theta] %4.3f [Phi] %4.3f [Psi] %4.3f\n",nd->theta,nd->phi,nd->psi);
printf("Altitude : %i\n",nd->altitude);
printf("Speed : [vX] %4.3f [vY] %4.3f [vZPsi] %4.3f\n",nd->theta,nd->phi,nd->psi);
printf("\033[8A");
return C_OK;
}
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 QCustomTabBarLineEdit::setupWidget()
{
connect(this, SIGNAL(editingFinished()), this, SLOT(publishText()));
setWindowFlags(Qt::CustomizeWindowHint | Qt::Popup);
}
