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); }