void parseScoringEvents(std::string page, std::vector<ScoringEvent> *scoringEvents, Game *game, League *league){ /* For goals, we dont' want anything above the scoring summary, or below the next </table> */ std::string goals = split(page, "Scoring Summary")[1]; goals = split(goals, "</table>")[0]; /* Split by </td> to break up the periods. The first and last elements are garbage, just toss them. */ std::vector<std::string> goalVec = split(goals, "</td>"); goalVec.erase(goalVec.begin()); goalVec.erase(goalVec.end()); /* Go through and remove the opening <td> tag now from every element, it's unnecessary. */ for(int i = 0; i < goalVec.size(); i++){ goalVec[i] = extract(goalVec[i] + "</td>", "td"); } /* Cycle through goals by period */ for(int i = 0; i < goalVec.size(); i+=2){ /* Only if there was a goal in that period */ if(goalVec[i+1].find("(no scoring)") == std::string::npos ){ /* Get the actual number for this period */ int per = getPeriod(goalVec[i]); /* If there are more than one goals in that period, split them up and process them seperately. If there is only one goal, process it directly. */ if(goalVec[i+1].find("<tr>") != std::string::npos){ std::vector<std::string> firstsplit = split(goalVec[i+1], "<tr>"); for(int n = 0; n < firstsplit.size(); n++){ /* If it wasn't the first goal of the period, there will be another <td> tag on it. We can't have that, so strip it off. */ if(firstsplit[n].find("<td") != std::string::npos) firstsplit[n] = extract(firstsplit[n], "td"); scoringEvents->push_back(processGoal(game, league, per, firstsplit[n])); } }else{ scoringEvents->push_back(processGoal(game, league, per, goalVec[i+1])); } } } }
// Action server sends goals here void goalCB() { ROS_INFO_STREAM_NAMED("pick_place_moveit","Received goal -----------------------------------------------"); pick_place_goal_ = action_server_.acceptNewGoal(); base_link_ = pick_place_goal_->frame; processGoal(pick_place_goal_->pickup_pose, pick_place_goal_->place_pose); }
// Skip perception void fake_goalCB() { ROS_INFO_STREAM_NAMED("pick_place_moveit","Received fake goal ----------------------------------------"); // Position geometry_msgs::Pose start_block_pose; geometry_msgs::Pose end_block_pose; // Does not work start_block_pose.position.x = 0.35; start_block_pose.position.y = 0.2; start_block_pose.position.z = 0.02; // Works - close start_block_pose.position.x = 0.2; start_block_pose.position.y = 0.0; start_block_pose.position.z = 0.02; // 3rd try start_block_pose.position.x = 0.35; start_block_pose.position.y = 0.1; start_block_pose.position.z = 0.02; nh_.param<double>("/block_pick_place_server/block_x", start_block_pose.position.x, 0.2); nh_.param<double>("/block_pick_place_server/block_y", start_block_pose.position.y, 0.0); nh_.param<double>("/block_pick_place_server/block_z", start_block_pose.position.z, 0.02); ROS_INFO_STREAM_NAMED("pick_place_moveit","start block is \n" << start_block_pose.position); end_block_pose.position.x = 0.25; end_block_pose.position.y = 0.15; end_block_pose.position.z = 0.02; // Orientation double angle = M_PI / 1.5; Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ())); start_block_pose.orientation.x = quat.x(); start_block_pose.orientation.y = quat.y(); start_block_pose.orientation.z = quat.z(); start_block_pose.orientation.w = quat.w(); angle = M_PI / 1.1; quat = Eigen::Quaterniond(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ())); end_block_pose.orientation.x = quat.x(); end_block_pose.orientation.y = quat.y(); end_block_pose.orientation.z = quat.z(); end_block_pose.orientation.w = quat.w(); // Fill goal base_link_ = "base_link"; processGoal(start_block_pose, end_block_pose); }