/**
* @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;
}
void JointMovementActionController::executeCB(const JMAS::GoalConstPtr &goal)
{
// note: the SimpleActionServer guarantees that we enter this function only when
// there is no other active goal. in other words, only one instance of executeCB()
// is ever running at the same time.
if (!suitableJointGoal(goal->jointGoal.name))
{
ROS_ERROR("Joints on incoming goal don't match our joints");
for (size_t i = 0; i < goal->jointGoal.name.size(); i++)
{
ROS_INFO(" incoming joint %d: %s", (int)i, goal->jointGoal.name[i].c_str());
}
for (size_t i = 0; i < joints_.size(); i++)
{
ROS_INFO(" our joint %d: %s", (int)i, joints_[i].c_str());
}
action_server_.setAborted();
return;
}
if (action_server_.isPreemptRequested())
{
ROS_WARN("New action goal already seems to have been cancelled!");
action_server_.setPreempted();
return;
}
// adjust all goal positions to match the given motor limits
sensor_msgs::JointState adjustedJointGoal = adjustJointGoalPositionsToMotorLimits(goal->jointGoal);
ROS_INFO("Sending movement to Crops arm...");
for (size_t i = 0; i < adjustedJointGoal.name.size(); i++)
{
if (!crops_->moveJoint(crops_->getJointIndex(adjustedJointGoal.name[i]), adjustedJointGoal.position[i]))
{
ROS_ERROR("Problem while transferring movement to Crops arm. Aborting...");
action_server_.setAborted();
return;
}
}
ros::Rate goalWait(10.0);
while (ros::ok())
{
// always have to call this before calling someMotorCrashed() or allJointsReady()
crops_->refreshMotorStatus();
if (crops_->someMotorCrashed())
{
ROS_ERROR("Some motor has crashed! Aborting...");
action_server_.setAborted();
return;
}
if (crops_->allJointsReady())
{
ROS_INFO("...movement successfully executed.");
action_server_.setSucceeded();
return;
}
if (action_server_.isPreemptRequested())
{
ROS_WARN("Goal canceled by client while waiting for movement to finish, aborting!");
action_server_.setPreempted();
return;
}
goalWait.sleep();
}
}
