void Server::listParameters()
{
HIGHLIGHT("************* Parameters *************");
for (auto & it : params_)
{
HIGHLIGHT("Parameter: "<<it.first);
}
HIGHLIGHT("**************************************");
}
void IMesh::initDebug(std::string ref)
{
imesh_mark_.scale.x = 0.005;
imesh_mark_.color.a = imesh_mark_.color.r = 1;
imesh_mark_.type = visualization_msgs::Marker::LINE_LIST;
imesh_mark_.header.frame_id = ref;
imesh_mark_.ns = getObjectName();
imesh_mark_pub_ = server_->advertise<visualization_msgs::Marker>(ns_ +"/InteractionMesh", 1, true);
HIGHLIGHT("InteractionMesh connectivity is published on ROS topic "<<imesh_mark_pub_.getTopic()<<", in reference frame "<<ref);
}
void *WorkScheduler::thread_execute_gpu(void *data)
{
Device *device = (Device *)data;
WorkPackage *work;
while ((work = (WorkPackage *)BLI_thread_queue_pop(g_gpuqueue))) {
HIGHLIGHT(work);
device->execute(work);
delete work;
}
return NULL;
}
void OMPLsolver::Solve(Eigen::VectorXdRefConst q0,
Eigen::MatrixXd & solution)
{
if (base_solver_->solve(q0, solution))
{
HIGHLIGHT(
"OMPL solving succeeded, planning time "<<base_solver_->getPlanningTime()<<"sec");
}
else
{
throw_solve("OMPL solving failed");
}
}
void *WorkScheduler::thread_execute_cpu(void *data)
{
CPUDevice *device = (CPUDevice *)data;
WorkPackage *work;
BLI_thread_local_set(g_thread_device, device);
while ((work = (WorkPackage *)BLI_thread_queue_pop(g_cpuqueue))) {
HIGHLIGHT(work);
device->execute(work);
delete work;
}
return NULL;
}
void Identity::initialise(std::string& postureName,
std::vector<std::string>& joints, bool skipUnknown)
{
if (!poses || !posesJointNames)
{
throw_named("Poses have not been set!");
}
std::map<std::string, Eigen::VectorXd>::const_iterator pose = poses->find(
postureName);
if (pose != poses->end())
{
useRef = true;
jointMap.resize(0);
jointRef.resize(0);
for (int i = 0; i < joints.size(); i++)
{
int idext = getJointIDexternal(joints[i]);
if (idext >= 0)
{
int id = getJointID(joints[i]);
if (id >= 0)
{
jointMap.push_back(id);
jointRef.conservativeResize(jointRef.rows() + 1);
jointRef(jointRef.rows() - 1) = pose->second(idext);
continue;
}
}
else
{
if (!skipUnknown)
{
throw_named("Requesting unknown joint '"<<joints[i]<<"'");
}
}
}
if (jointMap.size() == 0)
{
HIGHLIGHT("No joints have been specified!");
}
}
else
{
throw_named("Can't find pose '"<<postureName<<"'");
}
}
EReturn OMPLsolver::getSimplifiedPath(ompl::geometric::PathGeometric &pg,
Eigen::MatrixXd & traj, ob::PlannerTerminationCondition &ptc)
{
if (smooth_->data)
{
HIGHLIGHT("Simplifying solution");
int original_cnt = pg.getStateCount();
ros::Time start = ros::Time::now();
//ompl_simple_setup_->simplifySolution(d);
// Lets do our own simplifier ~:)
if (original_cnt >= 3)
{
og::PathSimplifierPtr psf_ = ompl_simple_setup_->getPathSimplifier();
const ob::SpaceInformationPtr &si =
ompl_simple_setup_->getSpaceInformation();
bool tryMore = false;
if (ptc == false) tryMore = psf_->reduceVertices(pg);
if (ptc == false) psf_->collapseCloseVertices(pg);
int times = 0;
while (tryMore && ptc == false)
{
tryMore = psf_->reduceVertices(pg);
times++;
}
if (si->getStateSpace()->isMetricSpace())
{
if (ptc == false)
tryMore = psf_->shortcutPath(pg);
else
tryMore = false;
times = 0;
while (tryMore && ptc == false)
{
tryMore = psf_->shortcutPath(pg);
times++;
}
}
std::vector<ob::State*> &states = pg.getStates();
// Calculate number of states required
unsigned int length = 0;
const int n1 = states.size() - 1;
for (int i = 0; i < n1; ++i)
length += si->getStateSpace()->validSegmentCount(states[i],
states[i + 1]);
// // Forward reducing
// HIGHLIGHT("States before forward reducing: "<<pg.getStateCount());
// pg.interpolate(length);
//
// bool need_backward = true;
// for (int i = states.size() - 1; i > 0; i--)
// {
// if (si->checkMotion(states[0], states[i]))
// {
// ob::State *start = si->cloneState(states[0]);
// pg.keepAfter(states[i]);
// pg.prepend(start);
// need_backward = (i == states.size() - 1) ? false : true;
// break;
// }
// }
//
// // Backward reducing
// ob::State *mid;
// if (need_backward)
// {
// HIGHLIGHT("States before backward reducing: "<<pg.getStateCount());
// pg.interpolate(length);
// for (int i = 1; i < states.size(); i++)
// {
// if (si->checkMotion(states[states.size() - 1], states[i]))
// {
// ob::State *goal = si->cloneState(states[states.size() - 1]);
// pg.keepBefore(states[i]);
// pg.append(goal);
// mid = si->cloneState(states[i]);
// break;
// }
// }
// }
pg.interpolate(length);
}
// if (ompl_simple_setup_->haveSolutionPath())
// {
// pg.interpolate();
// }
HIGHLIGHT_NAMED("OMPLSolver",
"Simplification took "<<ros::Duration(ros::Time::now()-start).toSec()<<"sec. States: "<<original_cnt<<"->"<<pg.getStateCount());
}
convertPath(pg, traj);
return SUCCESS;
}