bool isGoalReached(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, tf::Stamped<tf::Pose>& global_pose, const nav_msgs::Odometry& base_odom, double rot_stopped_vel, double trans_stopped_vel, double xy_goal_tolerance, double yaw_goal_tolerance){ //we assume the global goal is the last point in the global plan tf::Stamped<tf::Pose> goal_pose; getGoalPose(tf, global_plan, global_frame, goal_pose); double goal_x = goal_pose.getOrigin().getX(); double goal_y = goal_pose.getOrigin().getY(); double goal_th = tf::getYaw(goal_pose.getRotation()); //check to see if we've reached the goal position if(getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) { //check to see if the goal orientation has been reached if(fabs(getGoalOrientationAngleDifference(global_pose, goal_th)) <= yaw_goal_tolerance) { //make sure that we're actually stopped before returning success if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel)) return true; } } return false; }
bool handrailClimbing(R2Interface& interface, const ISSWorld& world) { // Creating a list of handrails to climb across bool leftLegFixed = false; std::vector<std::string> handrailPlacements; handrailPlacements.push_back("Handrail_deck_1"); // left handrailPlacements.push_back("Handrail_port_0"); // right handrailPlacements.push_back("Handrail_stbd_2"); // left // Allocating two states - the "current" state and the next state (the goal state) robot_state::RobotStatePtr currentState = interface.allocRobotState(); *(currentState.get()) = interface.getCurrentRobotState(); currentState->update(); // This is where we will store the trajectories as they are computed std::vector<moveit_msgs::RobotTrajectory> trajectories(handrailPlacements.size()); bool success = true; bool leftLegFixedFirst = leftLegFixed; for(size_t i = 0; i < handrailPlacements.size(); ++i) { // Fix collisions with handrails adjustAllowedHandrailCollisions(interface, currentState, world, leftLegFixed, handrailPlacements[i]); // Figure out what is moving where std::string fixedLink, movingLink; fixedLink = leftLegFixed ? LEFT_LEG_END_LINK : RIGHT_LEG_END_LINK; movingLink = leftLegFixed ? RIGHT_LEG_END_LINK : LEFT_LEG_END_LINK; std::cout << "Fixing link " << fixedLink << " and moving link " << movingLink << std::endl; Eigen::Affine3d goal_pose = getGoalPose(world, handrailPlacements[i]); geometry_msgs::PoseStamped pose_msg; tf::poseEigenToMsg(goal_pose, pose_msg.pose); pose_msg.header.frame_id = "virtual_world"; // fix the fixed leg for the whole path moveit_msgs::Constraints leg_constraint; leg_constraint.position_constraints.push_back(createPositionConstraint(fixedLink, *(currentState.get()), moveit_r2_kinematics::CRITICAL_PRIO_LINEAR_TOL)); leg_constraint.orientation_constraints.push_back(createOrientationConstraint(fixedLink, *(currentState.get()), moveit_r2_kinematics::CRITICAL_PRIO_ANGULAR_TOL)); // Setup goal pose constraint for moving foot moveit_msgs::PositionConstraint goal_pos_constraint; goal_pos_constraint.header.frame_id = "virtual_world"; goal_pos_constraint.link_name = movingLink; goal_pos_constraint.target_point_offset.x = 0; goal_pos_constraint.target_point_offset.y = 0; goal_pos_constraint.target_point_offset.z = 0; // Define a bounding box - the goal region shape_msgs::SolidPrimitive box; box.type = shape_msgs::SolidPrimitive::BOX; // Allow for wiggle room along the handrail // TODO: There is a bug here, so no wiggle room allowed. // Likely a problem with tolerances and the wiggle room. Is the frame correct?? const ISSWorld::Handrail* hr = world.getHandrail(handrailPlacements[i]); if (hr->group == "Handrail_deck" || hr->group == "Handrail_overhead") { box.dimensions.push_back(moveit_r2_kinematics::CRITICAL_PRIO_LINEAR_TOL); // BOX_X box.dimensions.push_back(moveit_r2_kinematics::CRITICAL_PRIO_LINEAR_TOL); // BOX_Y //box.dimensions.push_back(0.4); // BOX_Y - half width of handrail box.dimensions.push_back(moveit_r2_kinematics::CRITICAL_PRIO_LINEAR_TOL); // BOX_Z } else if (hr->group == "Handrail_port" || hr->group == "Handrail_starboard") { box.dimensions.push_back(moveit_r2_kinematics::CRITICAL_PRIO_LINEAR_TOL); // BOX_X box.dimensions.push_back(moveit_r2_kinematics::CRITICAL_PRIO_LINEAR_TOL); // BOX_Y //box.dimensions.push_back(0.4); // BOX_Z - half width of handrail box.dimensions.push_back(moveit_r2_kinematics::CRITICAL_PRIO_LINEAR_TOL); // BOX_Z } goal_pos_constraint.constraint_region.primitives.push_back(box); goal_pos_constraint.weight = 1.0; // Center the bounding box at the goal pose location goal_pos_constraint.constraint_region.primitive_poses.push_back(pose_msg.pose); // Create orientation constraint (upright over handrail) moveit_msgs::OrientationConstraint goal_orn_constraint; goal_orn_constraint.header.frame_id = pose_msg.header.frame_id; goal_orn_constraint.orientation = pose_msg.pose.orientation; goal_orn_constraint.link_name = movingLink; goal_orn_constraint.absolute_x_axis_tolerance = moveit_r2_kinematics::CRITICAL_PRIO_ANGULAR_TOL; goal_orn_constraint.absolute_y_axis_tolerance = moveit_r2_kinematics::CRITICAL_PRIO_ANGULAR_TOL; goal_orn_constraint.absolute_z_axis_tolerance = moveit_r2_kinematics::CRITICAL_PRIO_ANGULAR_TOL; goal_orn_constraint.weight = 1.0; // Amalgamate all goal constraints together std::vector<moveit_msgs::Constraints> goal_constraints(1); goal_constraints.back().position_constraints.push_back(goal_pos_constraint); goal_constraints.back().orientation_constraints.push_back(goal_orn_constraint); goal_constraints.back().orientation_constraints.push_back(createTorsoUpConstraint()); moveit_msgs::RobotState start_state; moveit::core::robotStateToRobotStateMsg(*(currentState.get()), start_state); unsigned int tries = 10; success = false; // Try a few times before giving up. Sometimes we get a crappy goal state for(size_t attempt = 0; attempt < tries && !success; ++attempt) { if (interface.plan(start_state, leg_constraint, goal_constraints, "legs", 10.0, "CBiRRT2", trajectories[i])) { ROS_WARN("Step %lu (to %s) is glorious success", i, handrailPlacements[i].c_str()); success = true; } else ROS_ERROR("Attempt %lu to '%s' brings shame upon you", attempt, handrailPlacements[i].c_str(), i); } if (!success) { ROS_ERROR("Too many planning failures. Giving up"); break; } // Extracting the next "start" state from the end of the most recently computed trajectory std::map<std::string, double> newValues; const trajectory_msgs::JointTrajectory& nextStateMsg = trajectories[i].joint_trajectory; for(size_t j = 0; j < nextStateMsg.joint_names.size(); ++j) newValues[nextStateMsg.joint_names[j]] = nextStateMsg.points.back().positions[j]; const trajectory_msgs::MultiDOFJointTrajectory& multiDOFTraj = trajectories[i].multi_dof_joint_trajectory; for(size_t j = 0; j < multiDOFTraj.joint_names.size(); ++j) { const geometry_msgs::Transform& tf = multiDOFTraj.points.back().transforms[j]; newValues[multiDOFTraj.joint_names[j] + std::string("/trans_x")] = tf.translation.x; newValues[multiDOFTraj.joint_names[j] + std::string("/trans_y")] = tf.translation.y; newValues[multiDOFTraj.joint_names[j] + std::string("/trans_z")] = tf.translation.z; newValues[multiDOFTraj.joint_names[j] + std::string("/rot_x")] = tf.rotation.x; newValues[multiDOFTraj.joint_names[j] + std::string("/rot_y")] = tf.rotation.y; newValues[multiDOFTraj.joint_names[j] + std::string("/rot_z")] = tf.rotation.z; newValues[multiDOFTraj.joint_names[j] + std::string("/rot_w")] = tf.rotation.w; } currentState->setVariablePositions(newValues); currentState->update(); // Assuming that the legs alternate being fixed leftLegFixed = !leftLegFixed; } if (success) { openAndCloseGrippers(trajectories, leftLegFixedFirst); interface.viewTrajectory(trajectories); ros::spinOnce(); sleep(1); } return success; }