void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh) { ROS_ERROR_NAMED("actionlib", "In callback"); //assign to our stored goal handle *gh_ = gh; TestGoal goal = *gh.getGoal(); switch (goal.goal) { case 1: gh.setAccepted(); gh.setSucceeded(TestResult(), "The ref server has succeeded"); break; case 2: gh.setAccepted(); gh.setAborted(TestResult(), "The ref server has aborted"); break; case 3: gh.setRejected(TestResult(), "The ref server has rejected"); break; default: break; } ros::shutdown(); }
void goalCB(GoalHandle gh) { // Ensures that the joints in the goal match the joints we are commanding. ROS_DEBUG("Received goal: goalCB"); ROS_INFO("Received goal: goalCB"); if (!is_joint_set_ok(joint_names_, gh.getGoal()->trajectory.joint_names)) { ROS_ERROR("Joints on incoming goal don't match our joints"); gh.setRejected(); return; } // Cancels the currently active goal. if (has_active_goal_) { ROS_DEBUG("Received new goal, canceling current goal"); // Stops the controller. trajectory_msgs::JointTrajectory empty; empty.joint_names = joint_names_; pub_controller_command_.publish(empty); // Marks the current goal as canceled. active_goal_.setCanceled(); has_active_goal_ = false; } gh.setAccepted(); active_goal_ = gh; has_active_goal_ = true; // Sends the trajectory along to the controller ROS_INFO("Publishing trajectory; sending commands to the joint controller "); current_traj_ = active_goal_.getGoal()->trajectory; pub_controller_command_.publish(current_traj_); }
void BaseTrajectoryActionController::goalCB(GoalHandle gh) { ROS_DEBUG("goalCB"); std::vector<std::string> joint_names(joints_.size()); for (size_t j = 0; j < joints_.size(); ++j) joint_names[j] = joints_[j]->name; // Ensures that the joints in the goal match the joints we are commanding. if (!setsEqual(joint_names, gh.getGoal()->trajectory.joint_names)) { ROS_ERROR("Joints on incoming goal don't match our joints"); gh.setRejected(); return; } // Cancels the currently active goal. if (active_goal_) { // Marks the current goal as canceled. active_goal_->setCanceled(); } starting(); // reset trajectory gh.setAccepted(); // Sends the trajectory along to the controller active_goal_ = boost::shared_ptr<GoalHandle>(new GoalHandle(gh)); commandTrajectory(share_member(gh.getGoal(),gh.getGoal()->trajectory), active_goal_); }
void CartesianImpedanceAction::goalCB(GoalHandle gh) { // cancel active goal if (active_goal_.isValid() && (active_goal_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)) { active_goal_.setCanceled(); } Goal g = gh.getGoal(); bool valid = true; for (size_t i = 0; i < g->trajectory.points.size(); i++) { valid = valid && checkImpedance(g->trajectory.points[i].impedance); } if (!valid) { cartesian_trajectory_msgs::CartesianImpedanceResult res; res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::INVALID_GOAL; gh.setRejected(res); } CartesianImpedanceTrajectory* trj_ptr = new CartesianImpedanceTrajectory; *trj_ptr = g->trajectory; CartesianImpedanceTrajectoryConstPtr trj_cptr = CartesianImpedanceTrajectoryConstPtr(trj_ptr); if (g->trajectory.header.stamp < rtt_rosclock::host_now()) { valid = false; cartesian_trajectory_msgs::CartesianImpedanceResult res; res.error_code = cartesian_trajectory_msgs::CartesianImpedanceResult::OLD_HEADER_TIMESTAMP; gh.setRejected(res); } if (valid) { port_cartesian_trajectory_command_.write(trj_cptr); gh.setAccepted(); active_goal_ = gh; } }
void goalCB(GoalHandle gh) { // Ensures that the joints in the goal match the joints we are commanding. ROS_DEBUG("Received goal: goalCB"); if (!setsEqual(joint_names_, gh.getGoal()->trajectory.joint_names)) { ROS_ERROR("Joints on incoming goal don't match our joints"); gh.setRejected(); return; } // Cancels the currently active goal. if (has_active_goal_) { ROS_DEBUG("Received new goal, canceling current goal"); // Stops the controller. trajectory_msgs::JointTrajectory empty; empty.joint_names = joint_names_; pub_controller_command_.publish(empty); // Marks the current goal as canceled. active_goal_.setCanceled(); has_active_goal_ = false; } // Sends the trajectory along to the controller if (withinGoalConstraints(last_controller_state_, goal_constraints_, gh.getGoal()->trajectory)) { ROS_INFO_STREAM("Already within goal constraints"); gh.setAccepted(); gh.setSucceeded(); } else { gh.setAccepted(); active_goal_ = gh; has_active_goal_ = true; ROS_INFO("Publishing trajectory"); current_traj_ = active_goal_.getGoal()->trajectory; pub_controller_command_.publish(current_traj_); } }
void cancelCB(GoalHandle gh) { ROS_ERROR("Action cancel not supported for grasp execution action"); gh.setRejected(); }
void goalCB(GoalHandle gh) { GripperMessage gMsg; SimpleMessage request; SimpleMessage reply; ROS_DEBUG("Received grasping goal"); while (!(robot_->isConnected())) { ROS_DEBUG("Reconnecting"); robot_->makeConnect(); } switch(gh.getGoal()->goal) { case GraspHandPostureExecutionGoal::PRE_GRASP: gh.setAccepted(); ROS_WARN("Pre-grasp is not supported by this gripper"); gh.setSucceeded(); break; case GraspHandPostureExecutionGoal::GRASP: case GraspHandPostureExecutionGoal::RELEASE: gh.setAccepted(); switch(gh.getGoal()->goal) { case GraspHandPostureExecutionGoal::GRASP: ROS_INFO("Executing a gripper grasp"); gMsg.init(GripperOperationTypes::CLOSE); break; case GraspHandPostureExecutionGoal::RELEASE: ROS_INFO("Executing a gripper release"); gMsg.init(GripperOperationTypes::OPEN); break; } gMsg.toRequest(request); this->robot_->sendAndReceiveMsg(request, reply); switch(reply.getReplyCode()) { case ReplyTypes::SUCCESS: ROS_INFO("Robot gripper returned success"); gh.setSucceeded(); break; case ReplyTypes::FAILURE: ROS_ERROR("Robot gripper returned failure"); gh.setCanceled(); break; } break; default: gh.setRejected(); break; } }
void InternalSpaceSplineTrajectoryAction::goalCB(GoalHandle gh) { if (!goal_active_) { trajectory_msgs::JointTrajectory* trj_ptr = new trajectory_msgs::JointTrajectory; Goal g = gh.getGoal(); control_msgs::FollowJointTrajectoryResult res; RTT::Logger::log(RTT::Logger::Debug) << "Received trajectory contain " << g->trajectory.points.size() << " points" << RTT::endlog(); // fill remap table for (unsigned int i = 0; i < numberOfJoints_; i++) { int jointId = -1; for (unsigned int j = 0; j < g->trajectory.joint_names.size(); j++) { if (g->trajectory.joint_names[j] == jointNames_[i]) { jointId = j; break; } } if (jointId < 0) { RTT::Logger::log(RTT::Logger::Error) << "Trajectory contains invalid joint" << RTT::endlog(); res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS; gh.setRejected(res, ""); return; } else { remapTable_[i] = jointId; } } // Sprawdzenie ograniczeń w jointach INVALID_GOAL bool invalid_goal = false; for (unsigned int i = 0; i < numberOfJoints_; i++) { for (int j = 0; j < g->trajectory.points.size(); j++) { if (g->trajectory.points[j].positions[i] > upperLimits_[remapTable_[i]] || g->trajectory.points[j].positions[i] < lowerLimits_[remapTable_[i]]) { RTT::Logger::log(RTT::Logger::Debug) << "Invalid goal [" << i << "]: " << upperLimits_[remapTable_[i]] << ">" << g->trajectory.points[j].positions[i] << ">" << lowerLimits_[remapTable_[i]] << RTT::endlog(); invalid_goal = true; } } } if (invalid_goal) { RTT::Logger::log(RTT::Logger::Debug) << "Trajectory contains invalid goal!" << RTT::endlog(); res.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; gh.setRejected(res, ""); goal_active_ = false; return; } // Remap joints trj_ptr->header = g->trajectory.header; trj_ptr->points.resize(g->trajectory.points.size()); for (unsigned int i = 0; i < g->trajectory.points.size(); i++) { trj_ptr->points[i].positions.resize( g->trajectory.points[i].positions.size()); for (unsigned int j = 0; j < g->trajectory.points[i].positions.size(); j++) { trj_ptr->points[i].positions[j] = g->trajectory.points[i].positions[remapTable_[j]]; } trj_ptr->points[i].velocities.resize( g->trajectory.points[i].velocities.size()); for (unsigned int j = 0; j < g->trajectory.points[i].velocities.size(); j++) { trj_ptr->points[i].velocities[j] = g->trajectory.points[i].velocities[remapTable_[j]]; } trj_ptr->points[i].accelerations.resize( g->trajectory.points[i].accelerations.size()); for (unsigned int j = 0; j < g->trajectory.points[i].accelerations.size(); j++) { trj_ptr->points[i].accelerations[j] = g->trajectory.points[i] .accelerations[remapTable_[j]]; } trj_ptr->points[i].time_from_start = g->trajectory.points[i] .time_from_start; } // Sprawdzenie czasu w nagłówku OLD_HEADER_TIMESTAMP if (g->trajectory.header.stamp < rtt_rosclock::host_now()) { RTT::Logger::log(RTT::Logger::Debug) << "Old header timestamp" << RTT::endlog(); res.error_code = control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP; gh.setRejected(res, ""); } trajectory_finish_time_ = g->trajectory.header.stamp + g->trajectory.points[g->trajectory.points.size() - 1].time_from_start; activeGoal_ = gh; goal_active_ = true; bool ok = true; RTT::TaskContext::PeerList peers = this->getPeerList(); for (size_t i = 0; i < peers.size(); i++) { RTT::Logger::log(RTT::Logger::Debug) << "Starting peer : " << peers[i] << RTT::endlog(); ok = ok && this->getPeer(peers[i])->start(); } if (ok) { trajectory_msgs::JointTrajectoryConstPtr trj_cptr = trajectory_msgs::JointTrajectoryConstPtr(trj_ptr); trajectory_ptr_port_.write(trj_cptr); gh.setAccepted(); goal_active_ = true; } else { gh.setRejected(); goal_active_ = false; } } else { gh.setRejected(); } }