int
sendCmdBill_ (int s, resCmd cmd, struct resCmdBill *cmdmsg, int *retsock,
struct timeval *timeout)
{
char *buf;
int i,bufsize,cc;
bufsize = 1024;
bufsize = bufsize + ALIGNWORD_(strlen(cmdmsg->cwd)) + sizeof(int);
for (i=0; cmdmsg->argv[i]; i++)
bufsize = bufsize + ALIGNWORD_(strlen(cmdmsg->argv[i])) + sizeof(int);
if ((buf=malloc(bufsize)) == NULL) {
lserrno = LSE_MALLOC;
return -1;
}
umask(cmdmsg->filemask = (int)umask(0));
cmdmsg->priority = 0;
if (getLimits(cmdmsg->lsfLimits) < 0) {
lserrno = LSE_LIMIT_SYS;
free(buf);
return -1;
}
cc=callRes_(s, cmd, (char *) cmdmsg, buf, bufsize, xdr_resCmdBill,
retsock, timeout, NULL);
free(buf);
return cc;
}
void initializeImage(const struct EmbStitchList_ * start, const struct EmbStitchList_ * stop = 0) {
getLimits(start, stop);
const int nrows = std::ceil((maxY-minY)*scale);
const int ncols = std::ceil((maxX-minX)*scale);
img = cv::Mat_<float>(nrows, ncols, 0.0);
mask = cv::Mat_<uint8_t>(nrows, ncols);
}
Edge* IncrementalBlueNoise::AddBoundingBox(vector<Node2d*>::iterator first,
vector<Node2d*>::iterator last)
{
double xmin, ymin, xmax, ymax = 0.0;
getLimits(first, last, xmin, ymin, xmax, ymax);
real max = 0;
max = xmax > ymax ? xmax : ymax;
const int size = 1;
Node2d* n1 = new Node2d(-size * max, 0);
Node2d* n2 = new Node2d(2*size * max, 0);
Node2d* n3 = new Node2d(0.5, size * max);
// diagonal
Edge* e1 = new Edge; // lower
Edge* e2 = new Edge; // upper, the twin edge
Edge* e3 = new Edge;
e1->setSourceNode(n1);
e2->setSourceNode(n2);
e3->setSourceNode(n3);
e1->setNextEdgeInFace(e2);
e2->setNextEdgeInFace(e3);
e3->setNextEdgeInFace(e1);
e1->setAsLeadingEdge();
addLeadingEdge(e1);
inited = true;
return e1;
}
//--------------------------------------------------------------------------------------------------
Edge* IncrementalBlueNoise::initTwoEnclosingTriangles(vector<Node2d*>::iterator first,
vector<Node2d*>::iterator last)
{
double xmin, ymin, xmax, ymax = 0.0;
getLimits(first, last, xmin, ymin, xmax, ymax);
// Add 10% of range:
double fac = 10.0;
double dx = (xmax - xmin) / fac;
double dy = (ymax - ymin) / fac;
Node2d* n1 = new Node2d(xmin - dx, ymin - dy);
Node2d* n2 = new Node2d(xmax + dx, ymin - dy);
Node2d* n3 = new Node2d(xmax + dx, ymax + dy);
Node2d* n4 = new Node2d(xmin - dx, ymax + dy);
// diagonal
Edge* e1d = new Edge; // lower
Edge* e2d = new Edge; // upper, the twin edge
// lower triangle
Edge* e11 = new Edge;
Edge* e12 = new Edge;
// upper triangle
Edge* e21 = new Edge; // upper upper
Edge* e22 = new Edge;
// lower triangle
e1d->setSourceNode(n3);
e1d->setNextEdgeInFace(e11);
e1d->setTwinEdge(e2d);
e1d->setAsLeadingEdge();
addLeadingEdge(e1d);
e11->setSourceNode(n1);
e11->setNextEdgeInFace(e12);
e12->setSourceNode(n2);
e12->setNextEdgeInFace(e1d);
// upper triangle
e2d->setSourceNode(n1);
e2d->setNextEdgeInFace(e21);
e2d->setTwinEdge(e1d);
e2d->setAsLeadingEdge();
addLeadingEdge(e2d);
e21->setSourceNode(n3);
e21->setNextEdgeInFace(e22);
e22->setSourceNode(n4);
e22->setNextEdgeInFace(e2d);
inited = true;
return e11;
}
bool Hardware::center(Pantilt pantilt)
{
int minX, maxX, minY, maxY;
if (!getLimits(pantilt, minX, maxX, minY, maxY))
{
qWarning() << "Could not get limits";
return false;
}
return targetAbsolute(pantilt, (minX+maxX)/2, (minY+maxY)/2, false);
}
void LogConCenPH::checkAllActive(){
int ak = getAK();
vector<double> lims;
for(int i = 1; i < (ak-1); i++){
if(getAK() > i){
lims = getLimits(actIndex[i]);
if(lims[0] > -slpTol){
removeActive(actIndex[i]);
}
}
}
}
bool ossimGmlSupportData::configureGmljp2V1(
ossimRefPtr<ossimXmlNode> node0, const ossimImageGeometry* geom )
{
bool success = true;
const ossimString BLANK = "";
ossimString gridHighString;
ossimString gridLowString;
getLimits( geom, gridHighString, gridLowString );
configureBounds( node0, geom );
ossimString path = "rectifiedGridDomain";
ossimRefPtr<ossimXmlNode> node2 = node0->addChildNode( path, BLANK );
path = "RectifiedGrid";
ossimRefPtr<ossimXmlNode> node2a = node2->addChildNode( path, BLANK );
ossimRefPtr<ossimXmlAttribute> attr1 = new ossimXmlAttribute();
ossimString name = "dimension";
ossimString value = "2";
attr1->setNameValue( name, value );
node2a->addAttribute( attr1 );
path = "limit";
ossimRefPtr<ossimXmlNode> node2a1 = node2a->addChildNode( path, BLANK );
path = "GridEnvelope";
ossimRefPtr<ossimXmlNode> node2a1a = node2a1->addChildNode( path, BLANK );
path = "low";
ossimRefPtr<ossimXmlNode> node2a1a1 = node2a1a->addChildNode( path, gridLowString );
path = "high";
ossimRefPtr<ossimXmlNode> node2a1a2 = node2a1a->addChildNode( path, gridHighString );
return success;
}
bool ossimGmlSupportData::configureGmljp2V2(
ossimRefPtr<ossimXmlNode> node0, const ossimImageGeometry* geom )
{
bool success = true;
const ossimString BLANK = "";
ossimRefPtr<ossimXmlAttribute> attr(0);
ossimString name;
ossimString value;
ossimString gridHighString;
ossimString gridLowString;
getLimits( geom, gridHighString, gridLowString );
configureBounds( node0, geom );
ossimString path = "domainSet";
ossimRefPtr<ossimXmlNode> node2 =
node0->addChildNode( path, BLANK );
path = "rangeSet";
ossimRefPtr<ossimXmlNode> node3 =
node0->addChildNode( path, BLANK );
path = "File";
ossimRefPtr<ossimXmlNode> node3a =
node3->addChildNode( path, BLANK );
path = "rangeParameters";
ossimRefPtr<ossimXmlNode> node3a1 =
node3a->addChildNode( path, BLANK );
path = "fileName";
ossimRefPtr<ossimXmlNode> node3a2 =
node3a->addChildNode( path, "gmljp2://codestream" );
path = "fileStructure";
ossimRefPtr<ossimXmlNode> node3a3 =
node3a->addChildNode( path, "inapplicable" );
path = "gmlcov:rangeType";
ossimRefPtr<ossimXmlNode> node4 =
node0->addChildNode( path, BLANK );
path = "gmljp2:featureMember";
ossimRefPtr<ossimXmlNode> node5 =
node0->addChildNode( path, BLANK );
path = "gmljp2:GMLJP2RectifiedGridCoverage";
ossimRefPtr<ossimXmlNode> node5a =
node5->addChildNode( path, BLANK );
attr = new ossimXmlAttribute();
name = "gml:id";
value = "CodeStream_0";
attr->setNameValue( name, value );
node5a->addAttribute( attr );
path = "domainSet";
ossimRefPtr<ossimXmlNode> node5a1 =
node5a->addChildNode( path, BLANK );
path = "RectifiedGrid";
ossimRefPtr<ossimXmlNode> node5a1a =
node5a1->addChildNode( path, BLANK );
attr = new ossimXmlAttribute();
name = "gml:id";
value = "RG0001";
attr->setNameValue( name, value );
node5a1a->addAttribute( attr );
attr = new ossimXmlAttribute();
name = "dimension";
value = "2";
attr->setNameValue( name, value );
node5a1a->addAttribute( attr );
attr = new ossimXmlAttribute();
name = "srsName";
attr->setNameValue( name, m_srsNameStringMap );
node5a1a->addAttribute( attr );
path = "limits";
ossimRefPtr<ossimXmlNode> node5a1a1 =
node5a1a->addChildNode( path, BLANK );
path = "GridEnvelope";
ossimRefPtr<ossimXmlNode> node5a1a1a =
node5a1a1->addChildNode( path, BLANK );
path = "low";
ossimRefPtr<ossimXmlNode> node5a1a1a1 =
node5a1a1a->addChildNode( path, gridLowString );
path = "high";
ossimRefPtr<ossimXmlNode> node5a1a1a2 =
node5a1a1a->addChildNode( path, gridHighString );
path = "axisLabels";
ossimRefPtr<ossimXmlNode> node5a1a2 =
node5a1a->addChildNode( path,
m_mapProj->isGeographic() ?
m_axisLabelsStringGeo : m_axisLabelsStringMap );
path = "origin";
ossimRefPtr<ossimXmlNode> node5a1a3 =
node5a1a->addChildNode( path, BLANK );
path = "Point";
ossimRefPtr<ossimXmlNode> node5a1a3a =
node5a1a3->addChildNode( path, BLANK );
attr = new ossimXmlAttribute();
name = "gml:id";
value = "P0001";
attr->setNameValue( name, value );
node5a1a3a->addAttribute( attr );
attr = new ossimXmlAttribute();
name = "srsName";
attr->setNameValue( name, m_srsNameStringMap );
node5a1a3a->addAttribute( attr );
ossimString originString;
ossimString offsetVector1String;
ossimString offsetVector2String;
getOrigin( geom, originString,
offsetVector1String,
offsetVector2String );
path = "pos";
ossimRefPtr<ossimXmlNode> node5a1a3a1 =
node5a1a3a->addChildNode( path, originString );
path = "offsetVector";
ossimRefPtr<ossimXmlNode> node5a1a4 =
node5a1a->addChildNode( path, offsetVector1String );
attr = new ossimXmlAttribute();
name = "srsName";
attr->setNameValue( name,
m_mapProj->isGeographic() ?
m_srsNameStringGeo : m_srsNameStringMap );
node5a1a4->addAttribute( attr );
path = "offsetVector";
ossimRefPtr<ossimXmlNode> node5a1a5 =
node5a1a->addChildNode( path, offsetVector2String );
attr = new ossimXmlAttribute();
name = "srsName";
attr->setNameValue( name,
m_mapProj->isGeographic() ?
m_srsNameStringGeo : m_srsNameStringMap );
node5a1a5->addAttribute( attr );
path = "rangeSet";
ossimRefPtr<ossimXmlNode> node5a2 =
node5a->addChildNode( path, BLANK );
path = "File";
ossimRefPtr<ossimXmlNode> node5a2a =
node5a2->addChildNode( path, BLANK );
path = "rangeParameters";
ossimRefPtr<ossimXmlNode> node5a2a1 =
node5a2a->addChildNode( path, BLANK );
path = "fileName";
ossimRefPtr<ossimXmlNode> node5a2a2 =
node5a2a->addChildNode( path, "gmljp2://codestream" );
path = "fileStructure";
ossimRefPtr<ossimXmlNode> node5a2a3 =
node5a2a->addChildNode( path, "inapplicable" );
return success;
}
int CmdCustom::execute (std::string& output)
{
int rc = 0;
// Load report configuration.
std::string reportColumns = context.config.get ("report." + _keyword + ".columns");
std::string reportLabels = context.config.get ("report." + _keyword + ".labels");
std::string reportSort = context.config.get ("report." + _keyword + ".sort");
std::string reportFilter = context.config.get ("report." + _keyword + ".filter");
std::vector <std::string> columns;
split (columns, reportColumns, ',');
validateReportColumns (columns);
std::vector <std::string> labels;
split (labels, reportLabels, ',');
if (columns.size () != labels.size () && labels.size () != 0)
throw format (STRING_CMD_CUSTOM_MISMATCH, _keyword);
std::map <std::string, std::string> columnLabels;
if (labels.size ())
for (unsigned int i = 0; i < columns.size (); ++i)
columnLabels[columns[i]] = labels[i];
std::vector <std::string> sortOrder;
split (sortOrder, reportSort, ',');
validateSortColumns (sortOrder);
// Prepend the argument list with those from the report filter.
std::vector <std::string> filterArgs;
split (filterArgs, reportFilter, ' ');
std::vector <std::string>::iterator arg;
for (arg = filterArgs.begin (); arg != filterArgs.end (); ++ arg)
context.a3.capture_first (*arg);
context.a3.categorize ();
// Load the data.
handleRecurrence ();
std::vector <Task> filtered;
filter (filtered);
// Sort the tasks.
std::vector <int> sequence;
for (unsigned int i = 0; i < filtered.size (); ++i)
sequence.push_back (i);
sort_tasks (filtered, sequence, reportSort);
// Configure the view.
ViewTask view;
view.width (context.getWidth ());
view.leftMargin (context.config.getInteger ("indent.report"));
view.extraPadding (context.config.getInteger ("row.padding"));
view.intraPadding (context.config.getInteger ("column.padding"));
Color label (context.config.get ("color.label"));
view.colorHeader (label);
Color alternate (context.config.get ("color.alternate"));
view.colorOdd (alternate);
view.intraColorOdd (alternate);
// Add the columns and labels.
for (unsigned int i = 0; i < columns.size (); ++i)
{
Column* c = Column::factory (columns[i], _keyword);
if (i < labels.size ())
c->setLabel (labels[i]);
view.add (c);
}
// How many lines taken up by table header?
// TODO Consider rc.verbose
int table_header = 0;
if (context.verbose ("label"))
{
if (context.color () && context.config.getBoolean ("fontunderline"))
table_header = 1; // Underlining doesn't use extra line.
else
table_header = 2; // Dashes use an extra line.
}
// Report output can be limited by rows or lines.
int maxrows = 0;
int maxlines = 0;
getLimits (_keyword, maxrows, maxlines);
// Adjust for fluff in the output.
if (maxlines)
maxlines -= (context.verbose ("blank") ? 1 : 0)
+ table_header
+ (context.verbose ("footnote") ? context.footnotes.size () : 0)
+ 1; // "X tasks shown ..."
// Render.
std::stringstream out;
if (filtered.size ())
{
view.truncateRows (maxrows);
view.truncateLines (maxlines);
out << optionalBlankLine ()
<< view.render (filtered, sequence)
<< optionalBlankLine ();
if (context.verbose ("affected"))
out << (filtered.size () == 1
? STRING_CMD_CUSTOM_COUNT
: format (STRING_CMD_CUSTOM_COUNTN, filtered.size ()));
// TODO Conditional
if (maxrows && maxrows < (int)filtered.size ())
out << ", " << format (STRING_CMD_CUSTOM_SHOWN, maxrows);
// TODO Conditional
if (maxlines && maxlines < (int)filtered.size ())
out << ", "
<< format (STRING_CMD_CUSTOM_TRUNCATED, maxlines - table_header);
if (context.verbose ("affected"))
out << "\n";
}
else
{
context.footnote (STRING_FEEDBACK_NO_MATCH);
rc = 1;
}
context.tdb2.commit ();
output = out.str ();
return rc;
}
bool ChompPlannerNode::filterJointTrajectory(arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request &request, arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response &res)
{
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request req = request;
ros::WallTime start_time = ros::WallTime::now();
ROS_INFO_STREAM("Received filtering request with trajectory size " << req.trajectory.points.size());
if(req.path_constraints.joint_constraints.size() > 0 ||
req.path_constraints.position_constraints.size() > 0 ||
req.path_constraints.orientation_constraints.size() > 0 ||
req.path_constraints.visibility_constraints.size() > 0) {
if(use_trajectory_filter_) {
ROS_INFO("Chomp can't handle path constraints, passing through to other trajectory filters");
if(!filter_trajectory_client_.call(req,res)) {
ROS_INFO("Pass through failed");
} else {
ROS_INFO("Pass through succeeded");
}
} else {
ROS_INFO("Chomp can't handle path constraints, and not set up to use additional filter");
}
return true;
}
for (unsigned int i=0; i< req.trajectory.points.size(); i++)
{
req.trajectory.points[i].velocities.resize(req.trajectory.joint_names.size(),0.0);
}
getLimits(req.trajectory, req.limits);
trajectory_msgs::JointTrajectory jtraj;
int num_points = req.trajectory.points.size();
if(num_points > maximum_spline_points_) {
num_points = maximum_spline_points_;
} else if(num_points < minimum_spline_points_) {
num_points = minimum_spline_points_;
}
//create a spline from the trajectory
spline_smoother::CubicTrajectory trajectory_solver;
spline_smoother::SplineTrajectory spline;
planning_environment::setRobotStateAndComputeTransforms(req.start_state,
*collision_proximity_space_->getCollisionModelsInterface()->getPlanningSceneState());
trajectory_solver.parameterize(req.trajectory,req.limits,spline);
double smoother_time;
spline_smoother::getTotalTime(spline, smoother_time);
ROS_INFO_STREAM("Total time given is " << smoother_time);
double t = 0.0;
vector<double> times(num_points);
for(int i = 0; i < num_points; i++,t += smoother_time/(1.0*(num_points-1))) {
times[i] = t;
}
spline_smoother::sampleSplineTrajectory(spline, times, jtraj);
//double planner_time = req.trajectory.points.back().time_from_start.toSec();
t = 0.0;
for(unsigned int i = 0; i < jtraj.points.size(); i++, t += smoother_time/(1.0*(num_points-1))) {
jtraj.points[i].time_from_start = ros::Duration(t);
}
ROS_INFO_STREAM("Sampled trajectory has " << jtraj.points.size() << " points with " << jtraj.points[0].positions.size() << " joints");
string group_name;
group_name = req.group_name;
vector<string> linkNames;
vector<string> attachedBodies;
collision_proximity_space_->setupForGroupQueries(group_name,
req.start_state,
linkNames,
attachedBodies);
ChompTrajectory trajectory(robot_model_, group_name, jtraj);
//configure the distance field - this should just use current state
arm_navigation_msgs::RobotState robot_state = req.start_state;
jointStateToArray(arm_navigation_msgs::createJointState(req.trajectory.joint_names, jtraj.points[0].positions), group_name, trajectory.getTrajectoryPoint(0));
//set the goal state equal to start state, and override the joints specified in the goal
//joint constraints
int goal_index = trajectory.getNumPoints()-1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
sensor_msgs::JointState goal_state = arm_navigation_msgs::createJointState(req.trajectory.joint_names, jtraj.points.back().positions);
jointStateToArray(goal_state, group_name,trajectory.getTrajectoryPoint(goal_index));
map<string, KinematicModel::JointModelGroup*> groupMap = robot_model_->getJointModelGroupMap();
KinematicModel::JointModelGroup* modelGroup = groupMap[group_name];
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
const KinematicModel::JointModel* model = modelGroup->getJointModels()[i];
const KinematicModel::RevoluteJointModel* revoluteJoint = dynamic_cast<const KinematicModel::RevoluteJointModel*>(model);
if (revoluteJoint != NULL)
{
if(revoluteJoint->continuous_)
{
double start = trajectory(0, i);
double end = trajectory(goal_index, i);
trajectory(goal_index, i) = start + angles::shortest_angular_distance(start, end);
}
}
}
//sets other joints
trajectory.fillInMinJerk();
trajectory.overwriteTrajectory(jtraj);
// set the max planning time:
chomp_parameters_.setPlanningTimeLimit(req.allowed_time.toSec());
chomp_parameters_.setFilterMode(true);
// optimize!
ChompOptimizer optimizer(&trajectory, robot_model_, group_name, &chomp_parameters_,
vis_marker_array_publisher_, vis_marker_publisher_, collision_proximity_space_);
optimizer.optimize();
// assume that the trajectory is now optimized, fill in the output structure:
vector<double> velocity_limits(trajectory.getNumJoints(), numeric_limits<double>::max());
res.trajectory.points.resize(trajectory.getNumPoints());
// fill in joint names:
res.trajectory.joint_names.resize(trajectory.getNumJoints());
for (size_t i = 0; i < modelGroup->getJointModels().size(); i++)
{
res.trajectory.joint_names[i] = modelGroup->getJointModels()[i]->getName();
velocity_limits[i] = joint_limits_[res.trajectory.joint_names[i]].max_velocity;
}
res.trajectory.header.stamp = ros::Time::now();
res.trajectory.header.frame_id = reference_frame_;
// fill in the entire trajectory
for (size_t i = 0; i < (unsigned int)trajectory.getNumPoints(); i++)
{
res.trajectory.points[i].positions.resize(trajectory.getNumJoints());
res.trajectory.points[i].velocities.resize(trajectory.getNumJoints());
for (size_t j=0; j < res.trajectory.points[i].positions.size(); j++)
{
res.trajectory.points[i].positions[j] = trajectory(i, j);
}
if (i==0)
res.trajectory.points[i].time_from_start = ros::Duration(0.0);
else
{
double duration = trajectory.getDiscretization();
// check with all the joints if this duration is ok, else push it up
for (int j=0; j < trajectory.getNumJoints(); j++)
{
double d = fabs(res.trajectory.points[i].positions[j] - res.trajectory.points[i-1].positions[j]) / velocity_limits[j];
if (d > duration)
duration = d;
}
try {
res.trajectory.points[i].time_from_start = res.trajectory.points[i-1].time_from_start + ros::Duration(duration);
} catch(...) {
ROS_INFO_STREAM("Potentially weird duration of " << duration);
}
}
}
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request next_req;
arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response next_res;
if(use_trajectory_filter_) {
next_req = req;
next_req.trajectory = res.trajectory;
next_req.allowed_time=ros::Duration(1.0);//req.allowed_time/2.0;
if(filter_trajectory_client_.call(next_req, next_res)) {
ROS_INFO_STREAM("Filter call ok. Sent trajectory had " << res.trajectory.points.size() << " points. Returned trajectory has " << next_res.trajectory.points.size() << " points ");
} else {
ROS_INFO("Filter call not ok");
}
res.trajectory = next_res.trajectory;
res.error_code = next_res.error_code;
res.trajectory.header.stamp = ros::Time::now();
res.trajectory.header.frame_id = reference_frame_;
} else {
res.error_code.val = res.error_code.val = res.error_code.SUCCESS;
}
// for every point in time:
for (unsigned int i=1; i<res.trajectory.points.size()-1; ++i)
{
double dt1 = (res.trajectory.points[i].time_from_start - res.trajectory.points[i-1].time_from_start).toSec();
double dt2 = (res.trajectory.points[i+1].time_from_start - res.trajectory.points[i].time_from_start).toSec();
// for every (joint) trajectory
for (int j=0; j < trajectory.getNumJoints(); ++j)
{
double dx1 = res.trajectory.points[i].positions[j] - res.trajectory.points[i-1].positions[j];
double dx2 = res.trajectory.points[i+1].positions[j] - res.trajectory.points[i].positions[j];
double v1 = dx1/dt1;
double v2 = dx2/dt2;
res.trajectory.points[i].velocities[j] = 0.5*(v1 + v2);
}
}
ROS_INFO("Serviced filter request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.points.back().time_from_start.toSec());
return true;
}
