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; }