/**
* @return either one of control_msgs::FollowJointTrajectoryResult, or PREEMPT_REQUESTED
*/
int JointTrajectoryActionController::executeCommon(const trajectory_msgs::JointTrajectory &trajectory,
boost::function<bool()> isPreemptRequested)
{
if (!setsEqual(joints_, trajectory.joint_names))
{
ROS_ERROR("Joints on incoming goal don't match our joints");
for (size_t i = 0; i < trajectory.joint_names.size(); i++)
{
ROS_INFO(" incoming joint %d: %s", (int)i, trajectory.joint_names[i].c_str());
}
for (size_t i = 0; i < joints_.size(); i++)
{
ROS_INFO(" our joint %d: %s", (int)i, joints_[i].c_str());
}
return control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
}
if (isPreemptRequested())
{
ROS_WARN("New action goal already seems to have been canceled!");
return PREEMPT_REQUESTED;
}
// make sure the katana is stopped
reset_trajectory_and_stop();
// ------ If requested, performs a stop
if (trajectory.points.empty())
{
// reset_trajectory_and_stop();
return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
}
// calculate new trajectory
boost::shared_ptr<SpecifiedTrajectory> new_traj = calculateTrajectory(trajectory);
if (!new_traj)
{
ROS_ERROR("Could not calculate new trajectory, aborting");
return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
}
if (!validTrajectory(*new_traj))
{
ROS_ERROR("Computed trajectory did not fulfill all constraints!");
return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
}
current_trajectory_ = new_traj;
// sleep until 0.5 seconds before scheduled start
ROS_DEBUG_COND(
trajectory.header.stamp > ros::Time::now(),
"Sleeping for %f seconds before start of trajectory", (trajectory.header.stamp - ros::Time::now()).toSec());
ros::Rate rate(10);
while ((trajectory.header.stamp - ros::Time::now()).toSec() > 0.5)
{
if (isPreemptRequested() || !ros::ok())
{
ROS_WARN("Goal canceled by client while waiting until scheduled start, aborting!");
return PREEMPT_REQUESTED;
}
rate.sleep();
}
ROS_INFO("Sending trajectory to Katana arm...");
bool success = katana_->executeTrajectory(new_traj, isPreemptRequested);
if (!success)
{
ROS_ERROR("Problem while transferring trajectory to Katana arm, aborting");
return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
}
ROS_INFO("Waiting until goal reached...");
ros::Rate goalWait(10);
while (ros::ok())
{
// always have to call this before calling someMotorCrashed() or allJointsReady()
katana_->refreshMotorStatus();
if (katana_->someMotorCrashed())
{
ROS_ERROR("Some motor has crashed! Aborting trajectory...");
return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
}
// all joints are idle
if (katana_->allJointsReady() && allJointsStopped())
{
// // make sure the joint positions are updated before checking for goalReached()
// --> this isn't necessary because refreshEncoders() is periodically called
// by KatanaNode. Leaving it out saves us some Katana bandwidth.
// katana_->refreshEncoders();
if (goalReached())
{
// joints are idle and we are inside goal constraints. yippie!
ROS_INFO("Goal reached.");
return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
}
else
{
ROS_ERROR("Joints are idle and motors are not crashed, but we did not reach the goal position! WTF?");
return control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
}
}
if (isPreemptRequested())
{
ROS_WARN("Goal canceled by client while waiting for trajectory to finish, aborting!");
return PREEMPT_REQUESTED;
}
goalWait.sleep();
}
// this part is only reached when node is shut down
return PREEMPT_REQUESTED;
}
huReturn hookUp(const vector<SplitEdge> &g, int s, vector<SplitEdge> &bIn, int k, vector<vector<int>> &Y, historyIndex &h)
{
if (cG(s, g) != 0)
{
cout << "cG(s) problem in hookup" << endl;
throw logic_error("");
}
vector<SplitEdge> H = g;
vector<SplitEdge> G1 = g;
vector<SplitEdge> B = bIn;
vector<SplitEdge> B1;
vector<vector<int>> XS;
int maxNodeInd = getMaxNodeInd(G1);
for (int i = 0; i < maxNodeInd; i++)
XS.push_back(vector<int>());
for (int i = 0; i < maxNodeInd; i++)
XS[i].push_back(i);
//cout << "About to enter while loop" << endl;
while (getNumUsedNodes(H) >= 4)
{
vector<int> ma = maOrderingHeap(H, s);
int v = ma[ma.size() - 2];
int w = ma[ma.size() - 1];
if (v == s || w == s)
throw logic_error("SET WAS V - S, S FOUND");
vector<int> X1;
H = combineVertices(H, v, w);
H = compress(H);
XS[v] = setUnion(XS[v], XS[w]);
if (XS[w].size() == 0)
{
cout << "Error: W, " << w << " was merged twice. Quitting" << endl;
throw logic_error("");
}
XS[w] = vector<int>();
if (cG(v, H) < k)
{
int numToGet = (int)ceil(.5*(double(k) - double(cG(G1, XS[v]))));
vector<SplitEdge> GX = inducedSubgraph(G1, XS[v]);
vector<SplitEdge> delB;
int added = 0;
for (unsigned i = 0; i < GX.size(); i++)
{
SplitEdge e = SplitEdge(GX[i].end0, GX[i].end1, GX[i].weight, GX[i].orig0, GX[i].orig1);
if (isMem(e, B))
{
int bW = B[indexOfEdge(B, e.end0, e.end1)].weight;
if (bW < e.weight)
e.weight = bW;
if (e.weight > (numToGet - added))
{
e.weight = numToGet - added;
}
added += e.weight;
delB.push_back(e);
}
if (added == numToGet)
break;
}
if (added != numToGet)
{
cout << "Error: GX did not contain " << numToGet << " entries in B. Quitting." << endl;
throw logic_error("");
}
if (!isSubset(delB, B))
{
cout << "ERROR: delB is not a subset of B." << endl;
cout << "B:" << endl;
output(B);
cout << "delB:" << endl;
output(delB);
cout << "This was the GX to choose from:" << endl;
output(GX);
cout << "V: " << v << endl;
cout << "W: " << w << endl;
cout << "S: " << s << endl;
throw logic_error("");
}
B = setRemove(delB, B);
B = removeZeroWeighted(B);
B1 = setUnion(delB, B1);
H = removeZeroWeighted(H);
G1 = hookUpHelper(s, G1, delB, h);
G1 = removeZeroWeighted(G1);
H = removeZeroWeighted(H);
bool addedFromXSinH = false;
numToGet *= 2;
for (unsigned i = 0; i < H.size(); i++)
{
SplitEdge tester = SplitEdge(s, v, 0, 0, 0);
if (equals(tester, H[i]))
{
//cout << "Increasing weight in hookUp in H between " << H[i].end0 << " and " << H[i].end1 << "from " << H[i].weight << " to " << H[i].weight + numToGet << endl;
H[i].weight += numToGet;
addedFromXSinH = true;
break;
}
}
if (!addedFromXSinH && numToGet != 0)
{
//cout << "Creating edge in hookUp in H between " << s << " and " << v << " with weight " << numToGet << endl;
SplitEdge e(s, v, numToGet, s, v);
H.push_back(e);
}
vector<vector<int>> newY;
for (unsigned i = 0; i < Y.size(); i++)
{
if (!isProperSubset(Y[i], XS[v]))
newY.push_back(Y[i]);
}
bool foundX1inY = false;
for (unsigned i = 0; i < newY.size(); i++)
{
if (setsEqual(newY[i], XS[v]))
foundX1inY = true;
}
if (!foundX1inY)
newY.push_back(XS[v]);
Y = newY;
}
}
huReturn ret;
ret.BP = B1;
ret.G1 = G1;
ret.Y = Y;
return ret;
}
