示例#1
0
 void TableApproaches::onInit() {
     table_appr_srv = nh.advertiseService("/table_detection/detect_table_approaches", &TableApproaches::tableApprCallback, this);
     costmap_ros = new costmap_2d::Costmap2DROS("table_costmap", tf_listener);
     traj_planner.initialize("table_traj_planner", &tf_listener, costmap_ros);
     footprint_model = costmap_ros->getRobotFootprint();
     valid_pub = nh.advertise<geometry_msgs::PoseArray>("valid_poses", 1);
     invalid_pub = nh.advertise<geometry_msgs::PoseArray>("invalid_poses", 1);
     approach_pub = nh.advertise<geometry_msgs::PoseArray>("approach_poses", 1);
     close_pub = nh.advertise<geometry_msgs::PoseArray>("close_poses", 1);
     
     //costmap_ros->start();
     ros::Duration(1.0).sleep();
 }
    HectorStuckRecoveryHandler(hector_move_base::IHectorMoveBase* interface) :
        HectorMoveBaseHandler(interface),
        costmap_(interface->getCostmap()),
        tf_(interface->getTransformListener()) {

        ros::NodeHandle nh;

        inverse_trajectory_service_client_ = nh.serviceClient<hector_nav_msgs::GetRecoveryInfo>("trajectory_recovery_info");
        inv_traj_back_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("inv_traj_back_pose",1);

        ros::NodeHandle controller_nh("controller");
        path_pub_ = controller_nh.advertise<hector_move_base_msgs::MoveBaseActionGeneric>("generic",1);
        path_drive_feedback_sub_ = controller_nh.subscribe("result", 5, &HectorStuckRecoveryHandler::resultCallback, this);

        world_model_ = new base_local_planner::CostmapModel(*costmap_->getCostmap());

        counter = 0;
    }
bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan, 
						const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame, 
						std::vector<geometry_msgs::PoseStamped>& transformed_plan, std::vector<int>& start_end_counts)
{
	const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

	// initiate refernce variables
	transformed_plan.clear();

	try
	{
		if (!global_plan.size() > 0)
		{
			ROS_ERROR("Recieved plan with zero length");
			return false;
		}

		tf::StampedTransform transform;
		tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, 
							plan_pose.header.frame_id, transform);

		//let's get the pose of the robot in the frame of the plan
		tf::Stamped<tf::Pose> robot_pose;
		robot_pose.setIdentity();
		robot_pose.frame_id_ = costmap.getBaseFrameID();
		robot_pose.stamp_ = ros::Time();
		tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);

		//we'll keep points on the plan that are within the window that we're looking at
		double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);

		unsigned int i = 0;
		double sq_dist_threshold = dist_threshold * dist_threshold;
		double sq_dist = DBL_MAX;

		// --- start - modification w.r.t. base_local_planner
		// initiate start_end_count
		std::vector<int> start_end_count;
		start_end_count.assign(2, 0);

		// we know only one direction and that is forward! - initiate search with previous start_end_counts
		// this is neccesserry to work with the sampling based planners - path may severall time enter and leave moving window
		ROS_ASSERT( (start_end_counts.at(0) > 0) && (start_end_counts.at(0) <= (int) global_plan.size()) );
		i = (unsigned int) global_plan.size() - (unsigned int) start_end_counts.at(0);
		// --- end - modification w.r.t. base_local_planner

		//we need to loop to a point on the plan that is within a certain distance of the robot
		while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold)
		{
			double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
			double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
			sq_dist = x_diff * x_diff + y_diff * y_diff;

			// --- start - modification w.r.t. base_local_planner
			// not yet in reach - get next frame
			if( sq_dist > sq_dist_threshold )
				++i;
			else
				// set counter for start of transformed intervall - from back as beginning of plan might be prunned
				start_end_count.at(0) = (int) (((unsigned int) global_plan.size()) - i);
			// --- end - modification w.r.t. base_local_planner
		}


		tf::Stamped<tf::Pose> tf_pose;
		geometry_msgs::PoseStamped newer_pose;

		//now we'll transform until points are outside of our distance threshold
		while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold)
		{
			double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
			double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
			sq_dist = x_diff * x_diff + y_diff * y_diff;

			const geometry_msgs::PoseStamped& pose = global_plan[i];
			poseStampedMsgToTF(pose, tf_pose);
			tf_pose.setData(transform * tf_pose);
			tf_pose.stamp_ = transform.stamp_;
			tf_pose.frame_id_ = global_frame;
			poseStampedTFToMsg(tf_pose, newer_pose);

			transformed_plan.push_back(newer_pose);

			// --- start - modification w.r.t. base_local_planner
			// set counter for end of transformed intervall - from back as beginning of plan might be prunned
			start_end_count.at(1) = int (((unsigned int) global_plan.size()) - i);
			// --- end - modification w.r.t. base_local_planner

			++i;
		}

		// --- start - modification w.r.t. base_local_planner
		// write to reference variable
		start_end_counts = start_end_count;
		// --- end - modification w.r.t. base_local_planner
    }
	catch(tf::LookupException& ex)
	{
		ROS_ERROR("No Transform available Error: %s\n", ex.what());
		return false;
	}
	catch(tf::ConnectivityException& ex)
	{
		ROS_ERROR("Connectivity Error: %s\n", ex.what());
		return false;
	}
	catch(tf::ExtrapolationException& ex)
	{
		ROS_ERROR("Extrapolation Error: %s\n", ex.what());
		if (global_plan.size() > 0)
			ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());

		return false;
	}

	return true;
}
bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
                         const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
                         std::vector<geometry_msgs::PoseStamped>& transformed_plan) {
    const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

    transformed_plan.clear();

    try {
        if (!global_plan.size() > 0)
        {
            ROS_ERROR("Recieved plan with zero length");
            return false;
        }

        tf::StampedTransform transform;
        tf.lookupTransform(global_frame, ros::Time(),
                           plan_pose.header.frame_id, plan_pose.header.stamp,
                           plan_pose.header.frame_id, transform);

        //let's get the pose of the robot in the frame of the plan
        tf::Stamped<tf::Pose> robot_pose;
        robot_pose.setIdentity();
        robot_pose.frame_id_ = costmap.getBaseFrameID();
        robot_pose.stamp_ = ros::Time();
        tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);

        //we'll keep points on the plan that are within the window that we're looking at
        double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);

        unsigned int i = 0;
        double sq_dist_threshold = dist_threshold * dist_threshold;
        double sq_dist = DBL_MAX;

        //we need to loop to a point on the plan that is within a certain distance of the robot
        while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold) {
            double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
            double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
            sq_dist = x_diff * x_diff + y_diff * y_diff;
            ++i;
        }

        //make sure not to count the first point that is too far away
        if(i > 0)
            --i;

        tf::Stamped<tf::Pose> tf_pose;
        geometry_msgs::PoseStamped newer_pose;

        //now we'll transform until points are outside of our distance threshold
        while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold) {
            double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
            double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
            sq_dist = x_diff * x_diff + y_diff * y_diff;

            const geometry_msgs::PoseStamped& pose = global_plan[i];
            poseStampedMsgToTF(pose, tf_pose);
            tf_pose.setData(transform * tf_pose);
            tf_pose.stamp_ = transform.stamp_;
            tf_pose.frame_id_ = global_frame;
            poseStampedTFToMsg(tf_pose, newer_pose);

            transformed_plan.push_back(newer_pose);

            ++i;
        }
    }
    catch(tf::LookupException& ex) {
        ROS_ERROR("No Transform available Error: %s\n", ex.what());
        return false;
    }
    catch(tf::ConnectivityException& ex) {
        ROS_ERROR("Connectivity Error: %s\n", ex.what());
        return false;
    }
    catch(tf::ExtrapolationException& ex) {
        ROS_ERROR("Extrapolation Error: %s\n", ex.what());
        if (global_plan.size() > 0)
            ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());

        return false;
    }

    return true;
}
bool isGoalReached(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
                   const costmap_2d::Costmap2DROS& costmap_ros, const std::string& global_frame,
                   const nav_msgs::Odometry& base_odom, double rot_stopped_vel, double trans_stopped_vel,
                   double xy_goal_tolerance, double yaw_goal_tolerance) {
    const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
    tf::Stamped<tf::Pose> goal_pose;

    try {
        if (!global_plan.size() > 0)
        {
            ROS_ERROR("Recieved plan with zero length");
            return false;
        }

        tf::StampedTransform transform;
        tf.lookupTransform(global_frame, ros::Time(),
                           plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
                           plan_goal_pose.header.frame_id, transform);

        poseStampedMsgToTF(plan_goal_pose, goal_pose);
        goal_pose.setData(transform * goal_pose);
        goal_pose.stamp_ = transform.stamp_;
        goal_pose.frame_id_ = global_frame;

    }
    catch(tf::LookupException& ex) {
        ROS_ERROR("No Transform available Error: %s\n", ex.what());
        return false;
    }
    catch(tf::ConnectivityException& ex) {
        ROS_ERROR("Connectivity Error: %s\n", ex.what());
        return false;
    }
    catch(tf::ExtrapolationException& ex) {
        ROS_ERROR("Extrapolation Error: %s\n", ex.what());
        if (global_plan.size() > 0)
            ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());

        return false;
    }

    //we assume the global goal is the last point in the global plan
    double goal_x = goal_pose.getOrigin().getX();
    double goal_y = goal_pose.getOrigin().getY();

    double yaw = tf::getYaw(goal_pose.getRotation());

    double goal_th = yaw;

    tf::Stamped<tf::Pose> global_pose;
    if(!costmap_ros.getRobotPose(global_pose))
        return false;

    //check to see if we've reached the goal position
    if(goalPositionReached(global_pose, goal_x, goal_y, xy_goal_tolerance)) {
        //check to see if the goal orientation has been reached
        if(goalOrientationReached(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;
}
示例#6
0
    bool TableApproaches::tableApprCallback(hrl_table_detect::GetTableApproaches::Request& req,
                                   hrl_table_detect::GetTableApproaches::Response& resp) {
        double pose_step = 0.01, start_dist = 0.25, max_dist = 1.2, min_cost = 250;
        double close_thresh = 0.10;


        // TODO should this be transformed?
        std::vector<geometry_msgs::Point> table_poly = req.table.points;
        geometry_msgs::PointStamped approach_pt = req.approach_pt;
        /*
        double xsize = 1.0, ysize = 1.0, xoff = 2.5, yoff = 0.0;
        geometry_msgs::Point pt; 
        pt.x = xoff-xsize/2; pt.y = yoff-ysize/2;
        table_poly.push_back(pt);
        pt.x = xoff+xsize/2; pt.y = yoff-ysize/2;
        table_poly.push_back(pt);
        pt.x = xoff+xsize/2; pt.y = yoff+ysize/2;
        table_poly.push_back(pt);
        pt.x = xoff-xsize/2; pt.y = yoff+ysize/2;
        table_poly.push_back(pt);
        geometry_msgs::PointStamped approach_pt;
        approach_pt.header.frame_id = "/base_link";
        approach_pt.header.stamp = ros::Time::now();
        approach_pt.point.x = 2.2; approach_pt.point.y = 0.3; 
        */

        costmap_ros->getCostmapCopy(costmap);
        world_model = new base_local_planner::CostmapModel(costmap);
        geometry_msgs::PoseArray valid_locs;
        valid_locs.header.frame_id = "/base_link";
        valid_locs.header.stamp = ros::Time::now();
        geometry_msgs::PoseArray invalid_locs;
        invalid_locs.header.frame_id = "/base_link";
        invalid_locs.header.stamp = ros::Time::now();
        geometry_msgs::PoseArray close_locs;
        close_locs.header.frame_id = "/base_link";
        close_locs.header.stamp = ros::Time::now();
        for(int i=0;i<4000;i++) {
            geometry_msgs::PoseStamped cpose, odom_pose;
            cpose.header.frame_id = "/base_link";
            cpose.header.stamp = ros::Time(0);
            cpose.pose.position.x = (rand()%8000) / 1000.0 -4 ; 
            cpose.pose.position.y = (rand()%8000) / 1000.0 - 4;
            double rot = (rand()%10000) / 10000.0 * 2 * 3.14;
            cpose.pose.orientation = tf::createQuaternionMsgFromYaw(rot);
            cout << cpose.pose.orientation.z << " " << cpose.pose.orientation.w << " " << rot << endl;
            tf_listener.transformPose("/odom_combined", cpose, odom_pose);
            //uint32_t x, y;
            //if(!costmap.worldToMap(odom_pose.pose.position.x, odom_pose.pose.position.y, x, y))
                //continue;
            Eigen::Vector3f pos(odom_pose.pose.position.x, odom_pose.pose.position.y, tf::getYaw(odom_pose.pose.orientation));
            //cout << x << ", " << y << ":" << curpt.point.x << "," << curpt.point.y << "$";
            //cout << double(costmap.getCost(x,y)) << endl;
            double foot_cost = footprintCost(pos, 1);
            if(foot_cost == 0) 
                valid_locs.poses.push_back(cpose.pose);
            else if(foot_cost != -1)
                close_locs.poses.push_back(cpose.pose);
            else 
                invalid_locs.poses.push_back(cpose.pose);
            cout << foot_cost << endl;
        }
        cout << costmap_ros->getRobotFootprint().size() << endl;
        valid_pub.publish(valid_locs);
        invalid_pub.publish(invalid_locs);
        close_pub.publish(close_locs);

        geometry_msgs::PoseArray dense_table_poses;
        dense_table_poses.header.frame_id = "/base_link";
        dense_table_poses.header.stamp = ros::Time::now();
        uint32_t i2;
        for(uint32_t i=0;i<table_poly.size();i++) {
            i2 = i+1;
            if(i2 == table_poly.size())
                i2 = 0;
            double diffx = table_poly[i2].x-table_poly[i].x;
            double diffy = table_poly[i2].y-table_poly[i].y;
            double len = std::sqrt(diffx*diffx + diffy*diffy);
            double ang = std::atan2(diffy, diffx) - 3.14/2;
            double incx = std::cos(ang)*0.01, incy = std::sin(ang)*0.01;
            for(double t=0;t<len;t+=pose_step) {
                double polyx = table_poly[i].x + t*diffx;
                double polyy = table_poly[i].y + t*diffy;
                geometry_msgs::PoseStamped test_pose, odom_test_pose;
                bool found_pose = false;
                for(int k=start_dist/0.01;k<max_dist/0.01;k++) {
                    test_pose.header.frame_id = "/base_link";
                    test_pose.header.stamp = ros::Time(0);
                    test_pose.pose.position.x = polyx + incx*k;
                    test_pose.pose.position.y = polyy + incy*k;
                    test_pose.pose.orientation = tf::createQuaternionMsgFromYaw(ang+3.14);
                    tf_listener.transformPose("/odom_combined", test_pose, odom_test_pose);
                    Eigen::Vector3f pos(odom_test_pose.pose.position.x, 
                                        odom_test_pose.pose.position.y, 
                                        tf::getYaw(odom_test_pose.pose.orientation));
                    double foot_cost = footprintCost(pos, 1.0);
                    // found a valid pose
                    if(foot_cost >= 0 && foot_cost <= min_cost) {
                        found_pose = true;
                        break;
                    }
                    uint32_t mapx, mapy;
                    // break if we go outside the grid
                    if(!costmap.worldToMap(odom_test_pose.pose.position.x, 
                                           odom_test_pose.pose.position.y, mapx, mapy))
                        break;
                    double occ_map = double(costmap.getCost(mapx, mapy));
                    // break if we come across and obstacle
                    if(occ_map == costmap_2d::LETHAL_OBSTACLE ||
                       occ_map == costmap_2d::NO_INFORMATION)
                        break;
                }
                if(found_pose)
                    dense_table_poses.poses.push_back(test_pose.pose);
            }
        }
        ROS_INFO("POLY: %d, denseposes: %d", table_poly.size(), dense_table_poses.poses.size());

        // downsample and sort dense pose possibilties by distance to
        // approach point and thresholded distance to each other
        geometry_msgs::PoseArray downsampled_table_poses;
        boost::function<bool(geometry_msgs::Pose&, geometry_msgs::Pose&)> dist_comp
                          = boost::bind(&pose_dist_comp, _1, _2, approach_pt.point);
        while(ros::ok() && !dense_table_poses.poses.empty()) {
            // add the closest valid pose to the approach location on the table
            geometry_msgs::Pose new_pose = *std::min_element(
                        dense_table_poses.poses.begin(), dense_table_poses.poses.end(), 
                        dist_comp);
            downsampled_table_poses.poses.push_back(new_pose);
            // remove all poses in the dense sampling which are close to
            // the newest added pose
            boost::function<bool(geometry_msgs::Pose)> rem_thresh
                          = boost::bind(&pose_dist_thresh, _1, new_pose.position, 
                                        close_thresh);
            dense_table_poses.poses.erase(std::remove_if(
                                          dense_table_poses.poses.begin(), 
                                          dense_table_poses.poses.end(),
                                          rem_thresh),
                                          dense_table_poses.poses.end());
            ROS_INFO("denseposes: %d", dense_table_poses.poses.size());
        }
        downsampled_table_poses.header.frame_id = "/base_link";
        downsampled_table_poses.header.stamp = ros::Time::now();
        approach_pub.publish(downsampled_table_poses);
        resp.approach_poses = downsampled_table_poses;
        ROS_INFO("POLY: %d, poses: %d", table_poly.size(), downsampled_table_poses.poses.size());

        delete world_model;
        return true;
    }