void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg){
	//ROS_INFO("map update");
	if(planning)
		return;
	//inflate
	//update local map
	local_map=occupancy_grid_utils::inflateObstacles(*msg,inflation_radius,true);
	map_pub.publish(local_map);
	mapChanged=true;
	if(!checkFeasibility()){
		plan();
	}
	sendWaypoint();
}
void SetGripperRotationPosition::execute()
{
    if (initialize())
    {
        if (checkFeasibility())
        {
            new std::thread(&SetGripperRotationPosition::executionWorker, this);
            this_thread::sleep_for(std::chrono::milliseconds(100));
            TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~%s~%s~%s~%s", getTaskId(), getTaskName().c_str(), (blocking) ? "blocking" : "non blocking", ((stopped) ? "stopped" : ((completed) ? "completed" : ((executing) ? "executing" : "queued"))), getTaskScriptLine().c_str()));
            return;
        }
    }
    this_thread::sleep_for(std::chrono::milliseconds(100));
    stopExecution();
    setCompleted();
}
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
	//ROS_INFO_STREAM("pose update: "<< msg->pose.position.x << "," << msg->pose.position.y);
	if(planning)
		return;
	//update local pose
	 pose.header.frame_id=msg->header.frame_id;
	 pose.header.stamp=ros::Time(0);
	 pose.point.x=msg->pose.position.x;
	 pose.point.y=msg->pose.position.y;

	startState[0]=pose.point.x;
	startState[1]=pose.point.y;

	if(!checkFeasibility()){
		plan();
	}
	sendWaypoint();
}
示例#4
0
LPSolver::Status LPSolver::optimize(
    OptimizationGoal goal,        // goal of optimization (minimize or maximize)
    Array<double> &obj,           // objective function vector
    Array<int>    &matrixBegin,   // matrixBegin[i] = begin of column i
    Array<int>    &matrixCount,   // matrixCount[i] = number of nonzeroes in column i
    Array<int>    &matrixIndex,   // matrixIndex[n] = index of matrixValue[n] in its column
    Array<double> &matrixValue,	  // matrixValue[n] = non-zero value in matrix
    Array<double> &rightHandSide, // right-hand side of LP constraints
    Array<char>   &equationSense, // 'E' ==   'G' >=   'L' <=
    Array<double> &lowerBound,    // lower bound of x[i]
    Array<double> &upperBound,    // upper bound of x[i]
    double &optimum,              // optimum value of objective function (if result is lpOptimal)
    Array<double> &x              // x-vector of optimal solution (if result is lpOptimal)
)
{
    if(osi->getNumCols()>0) { // get a fresh one if necessary
        delete osi;
        osi = CoinManager::createCorrectOsiSolverInterface();
    }

    const int numRows = rightHandSide.size();
    const int numCols = obj.size();
#ifdef OGDF_DEBUG
    const int numNonzeroes = matrixIndex.size();
#endif

    // assert correctness of array boundaries
    OGDF_ASSERT(obj          .low() == 0 && obj          .size() == numCols);
    OGDF_ASSERT(matrixBegin  .low() == 0 && matrixBegin  .size() == numCols);
    OGDF_ASSERT(matrixCount  .low() == 0 && matrixCount  .size() == numCols);
    OGDF_ASSERT(matrixIndex  .low() == 0 && matrixIndex  .size() == numNonzeroes);
    OGDF_ASSERT(matrixValue  .low() == 0 && matrixValue  .size() == numNonzeroes);
    OGDF_ASSERT(rightHandSide.low() == 0 && rightHandSide.size() == numRows);
    OGDF_ASSERT(equationSense.low() == 0 && equationSense.size() == numRows);
    OGDF_ASSERT(lowerBound   .low() == 0 && lowerBound   .size() == numCols);
    OGDF_ASSERT(upperBound   .low() == 0 && upperBound   .size() == numCols);
    OGDF_ASSERT(x            .low() == 0 && x            .size() == numCols);

    osi->setObjSense(goal==lpMinimize ? 1 : -1);

    int i;

    CoinPackedVector zero;
    for(i = 0; i < numRows; ++i) {
        osi->addRow(zero,equationSense[i],rightHandSide[i],0);
    }
    for(int colNo = 0; colNo < numCols; ++colNo) {
        CoinPackedVector cpv;
        for(i = matrixBegin[colNo]; i<matrixBegin[colNo]+matrixCount[colNo]; ++i) {
            cpv.insert(matrixIndex[i],matrixValue[i]);
        }
        osi->addCol(cpv,lowerBound[colNo],upperBound[colNo],obj[colNo]);
    }


    osi->initialSolve();

    Status status;
    if(osi->isProvenOptimal()) {
        optimum = osi->getObjValue();
        const double* sol = osi->getColSolution();
        for(i = numCols; i-->0;)
            x[i]=sol[i];
        status = lpOptimal;
        OGDF_ASSERT_IF(dlExtendedChecking,
                       checkFeasibility(matrixBegin,matrixCount,matrixIndex,matrixValue,
                                        rightHandSide,equationSense,lowerBound,upperBound,x));

    } else if(osi->isProvenPrimalInfeasible())
        status = lpInfeasible;
    else if(osi->isProvenDualInfeasible())
        status = lpUnbounded;
    else
        OGDF_THROW_PARAM(AlgorithmFailureException,afcNoSolutionFound);

    return status;
}