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