void Bug::advance(int step)
{
if (!step)
return;
frame += 1;
moveBy(cos(angle * PI / 180) * speed * BASE_SPEED, sin(angle * PI / 180) * speed * BASE_SPEED);
QPoint currentSquare = parent->square(*this);
if (currentSquare != lastSquare) {
lastSquare = currentSquare;
angle = parent->getAngle(currentSquare);
setRotation(angle);
}
if (currentSquare == parent->goal())
{
emit goalReached(this);
}
}
/**
* @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;
}
