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