C++ Duration::sleep方法代码示例

bool AriaRobotControl::wait_until_stopped(ros::Duration wait, int  max_iterations)
{
  wait.sleep();
  int count = 0;
  //get the state of the robot
  while(!get_state_client.call(get_state_srv));
  
  //std::cout << "IS STOPPED: " <<  get_state_srv.response.isStopped << std::endl; 
  //wait until the robot has stopped moving to issue a move command
  while((!(get_state_srv.response.isStopped) || !(get_state_srv.response.isHeadingDone) \
        || !(get_state_srv.response.isMoveDone)) && count < max_iterations)
  {
    ROS_INFO("ROBOT STILL moving");
    wait.sleep();
    while(!get_state_client.call(get_state_srv)){
      ROS_INFO("failed service call!");
    }
    count++;
  } 
  if(get_state_srv.response.isStopped)
  {
    return true;
  } 
  else
  {
    return false;
  }
}//end is robot stopped

void testForPose(moveit::planning_interface::MoveGroup &group,
		geometry_msgs::Pose initial_pose) {
	geometry_msgs::Pose destination;
	BottlePosition result;

	if (!moveTo(group, initial_pose)) {
		ROS_ERROR("No plan was found!");
		return;
	}
	stabilisation_duration.sleep();

	bool found_bottle = getBottlePositionFromTf(result);
	geometry_msgs::Point bottleCapPosition = result.bottleCapPose.position;
	ROS_INFO("Found bottle cap at: %f %f %f", bottleCapPosition.x,
			bottleCapPosition.y, bottleCapPosition.z);

	destination.position.x = bottleCapPosition.x;
	destination.position.y = bottleCapPosition.y;
	destination.position.z = bottleCapPosition.z + 0.03;
	destination.orientation = initial_pose.orientation;

	if (!moveTo(group, destination)) {
		ROS_ERROR("No plan was found!");
		return;
	}

	stabilisation_duration.sleep();

	if (!moveTo(group, initial_pose)) {
		ROS_ERROR("No plan was found!");
		return;
	}
	stabilisation_duration.sleep();
}

int Navigator::navigate_to_table() { 
    std::vector<geometry_msgs::PoseStamped> points;
    mobot_pub_des_state::path path_srv;
    points.push_back(trajBuilder_.xyPsi2PoseStamped(3.0,0.0,0.0));
    path_srv.request.path.poses = points;
    append_client_.call(path_srv);
    t_ = 0;
    looptimer.sleep();
    mobot_pub_des_state::emptyQueue emptyQueueMsg;
    empty_client_.call(emptyQueueMsg);
    while (!emptyQueueMsg.response.empty) {
        if (t_ > 25) {
            return navigator::navigatorResult::FAILED_CANNOT_REACH_DES_POSE;
        }
        t_ += 1.0;
        looptimer.sleep();
        empty_client_.call(emptyQueueMsg); 
    }
    return navigator::navigatorResult::DESIRED_POSE_ACHIEVED; //just say we were successful
}

/** This is a workaround for the case that we're running inside of
    rospy and ros::Time is not initialized inside the c++ instance. 
    This makes the system fall back to Wall time if not initialized.  
    https://github.com/ros/geometry/issues/30
*/
void sleep_fallback_to_wall(const ros::Duration& d)
{
  try
  {
      d.sleep();
  }
  catch (ros::TimeNotInitializedException ex)
  {
    ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec); 
    wd.sleep();
  }
}

BottlePosition findBottlePose(moveit::planning_interface::MoveGroup &group) {
	BottlePosition result;
	vector<Pose> arm_poses = computeArmPosesForDetection();
	vector<BottlePosition> measurements;

	for (vector<Pose>::iterator pose = arm_poses.begin();
			pose != arm_poses.end(); pose++) {
		if (!moveTo(group, *pose)) {
			throw std::runtime_error(
					"Error in determining bottle pose: The arm could not move to the given pose.");
		}
		stabilisation_duration.sleep();

		BottlePosition bottle_position;
		bool found_bottle = getBottlePositionFromTf(bottle_position);
		if (found_bottle) {
			measurements.push_back(bottle_position);

			ROS_INFO("Found bottle at:");
			printPose(measurements.back().bottlePose);
			ROS_INFO("And cap at:");
			printPose(measurements.back().bottleCapPose);
		} else {
			ROS_WARN("Measurement is ignored");
		}
	}
	if (measurements.size() == 0) {
		ROS_ERROR("The bottle was not found from any pose. The program will shut down!");
		exit(-1);
	}
	result = computeBottlePoseFromMeasurements(measurements);

	ROS_INFO("Average pose:");
	printPose(result.bottlePose);
	ROS_INFO("Average cap pose:");
	printPose(result.bottleCapPose);

	return result;
}

int main(int argc, char **argv)
{
    const double radius = 0.5; // 0.5
    const double pitch_step = 0.2;
    const int circle_points = 6; // 6
    const int circle_rings = 3; // 3
    const double plan_time = 5;
    const int plan_retry = 1;
    //const int pose_id_max = circle_points * 3;
    const ros::Duration wait_settle(1.0);
    const ros::Duration wait_camera(0.5);
    const ros::Duration wait_notreached(3.0);
    const ros::Duration wait_reset(30.0);

    const std::string planning_group = "bumblebee"; //robot, bumblebee
    const std::string end_effector = "tool_flange"; // bumblebee_cam1, tool_flange

    const std::string plannerId = "PRMkConfigDefault";
    const std::string frame_id = "base_link";

    ros::init (argc, argv, "simple_pose_mission");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle node_handle;
    ros::Publisher cameratate_pub = node_handle.advertise<group4_msgs::PointCloudPose>("/robot_rx60b/camerapose", 5);
    ros::Publisher markerPublisher = node_handle.advertise<visualization_msgs::MarkerArray>("/simple_pose_mission/poses", 10);

    tf::TransformListener listener;

    // Marker
    MarkerPose marker("object_center");
    addObjectCenterMarker(marker);

    // Robot and scene
    moveit::planning_interface::MoveGroup group(planning_group);
    group.setPlannerId(plannerId);
    group.setPoseReferenceFrame(frame_id);
    //group.setWorkspace(-1.5,-1.5,1.5,1.5,0,1);
    group.setGoalTolerance(0.01f);
    group.setPlanningTime(plan_time);

    group4_msgs::PointCloudPose msg;
    msg.header.frame_id = frame_id;

    tf::Pose object_center = marker.getPose("object_center");
    NumberedPoses poses = generatePoses(object_center, circle_points, circle_rings, pitch_step, radius);
    NumberedPoses::iterator poses_itt;
    bool poses_updated = true;


    while (ros::ok())
    {
        bool success;

        if (poses_updated || poses_itt == poses.end())
        {
            poses_itt = poses.begin();
            poses_updated = false;
        }

        NumberedPose numbered_pose = *poses_itt;

        visualizePoses(poses, frame_id,markerPublisher, numbered_pose.pose_id);


        ROS_INFO("Moving to pose %i of %i", numbered_pose.pose_id, numbered_pose.pose_id_max);



        tf::StampedTransform transform;
        try{
          listener.lookupTransform("/bumblebee_cam1", "/tool_flange",
                                   ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
          ROS_ERROR("%s",ex.what());
        }

        tf::Pose p = numbered_pose.pose_tf * transform;
        geometry_msgs::Pose desired_pose = tfPoseToGeometryPose(p);
        group.setPoseTarget(desired_pose, end_effector);

        success = false;
        for (int itry = 0; itry < plan_retry; itry++)
        {

            group.setStartStateToCurrentState();
            wait_notreached.sleep();
            success = group.move();
            if (success)
                break;
            else
                ROS_INFO("Plan unsuccessfull.. Retrying! (%i)", itry);
        }

        if (true)
        {

            wait_settle.sleep();

//.........这里部分代码省略.........