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