Line data    Source code

       1             : /*
       2             :  * Copyright 2020 <copyright holder> <email>
       3             :  *
       4             :  * Licensed under the Apache License, Version 2.0 (the "License");
       5             :  * you may not use this file except in compliance with the License.
       6             :  * You may obtain a copy of the License at
       7             :  *
       8             :  *     http://www.apache.org/licenses/LICENSE-2.0
       9             :  *
      10             :  * Unless required by applicable law or agreed to in writing, software
      11             :  * distributed under the License is distributed on an "AS IS" BASIS,
      12             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      13             :  * See the License for the specific language governing permissions and
      14             :  * limitations under the License.
      15             :  */
      16             : 
      17             : #include <ros_end_effector/RosActionServer.h>
      18             : 
      19          41 : ROSEE::RosActionServer::RosActionServer (std::string topicForAction, ros::NodeHandle* nh) :
      20          41 :     _actionServer(*nh, topicForAction, false) {
      21             :     
      22          41 :     this->nh = nh;
      23          41 :     this->topicForAction = topicForAction;
      24          41 :     goalInExecution = false;
      25          41 :     newGoal = false;
      26             :     
      27          41 :     _actionServer.registerGoalCallback (boost::bind(&RosActionServer::goalReceivedCallback, this));
      28          41 :     _actionServer.registerPreemptCallback (boost::bind (&RosActionServer::preemptReceivedCallback, this));
      29             :     
      30          41 :     _actionServer.start();
      31             :     
      32          41 : }
      33             : 
      34       21691 : bool ROSEE::RosActionServer::hasGoal () const {
      35       21691 :     return goalInExecution;
      36             : }
      37             : 
      38       21724 : bool ROSEE::RosActionServer::hasNewGoal () const {
      39       21724 :     return newGoal;
      40             : }
      41             : 
      42        2230 : double ROSEE::RosActionServer::getWantedNormError() const {
      43        2230 :     return wantedNormError;
      44             : }
      45             : 
      46          33 : rosee_msg::ROSEEActionControl ROSEE::RosActionServer::getGoal() {
      47          33 :     if (goalInExecution) {
      48          33 :         newGoal = false;
      49          33 :         return actionControlMsg;
      50             :     } else {
      51           0 :         ROS_WARN_STREAM ("ROSACTION SERVER no goal is in execution (no one has" <<
      52             :             "arrived or the last one is already completed. EMPTY GOAL RETURNING");
      53           0 :         return rosee_msg::ROSEEActionControl();
      54             :     }
      55             : }
      56             : 
      57             : 
      58          33 : void ROSEE::RosActionServer::goalReceivedCallback() {
      59             :     
      60             :     //ROS_INFO_STREAM (goal_action.
      61          33 :     goalInExecution = true;
      62          33 :     newGoal = true;
      63          33 :     this->actionControlMsg = _actionServer.acceptNewGoal()->goal_action;
      64             :     
      65          33 :     wantedNormError = actionControlMsg.error_norm == 0 ? DEFAULT_ERROR_NORM : actionControlMsg.error_norm;
      66             : 
      67          33 :     ROS_INFO_STREAM ("ROSACTION SERVER received goal: '" << this->actionControlMsg.action_name  << "'");
      68             : 
      69             :     
      70          33 : }
      71             : 
      72           0 : void ROSEE::RosActionServer::preemptReceivedCallback() {
      73             :     
      74           0 :     ROS_INFO_STREAM("ROSACTION SERVER Preempted old goal");
      75           0 :     goalInExecution = false;
      76           0 :     newGoal = false;
      77           0 :     wantedNormError = DEFAULT_ERROR_NORM;
      78             :     // set the action state to preempted
      79           0 :     _actionServer.setPreempted();
      80             :     
      81           0 : }
      82             : 
      83           0 : void ROSEE::RosActionServer::abortGoal(std::string errorMsg) {
      84           0 :     ROS_INFO_STREAM("ROSACTION SERVER Aborted goal");
      85           0 :     rosee_msg::ROSEECommandResult result;
      86           0 :     result.completed_action = actionControlMsg;
      87           0 :     _actionServer.setAborted(result, errorMsg);
      88           0 :     newGoal = false;
      89           0 :     goalInExecution = false;
      90           0 :     wantedNormError = DEFAULT_ERROR_NORM;
      91             : 
      92             : 
      93           0 : }
      94             : 
      95        2230 : void ROSEE::RosActionServer::sendFeedback(double completation_percentage, std::string currentAction) {
      96             :     
      97        2230 :     ROS_INFO_STREAM("ROSACTION SERVER Sending feedback " << completation_percentage );
      98             : 
      99        4460 :     rosee_msg::ROSEECommandFeedback feedback;
     100        2230 :     feedback.completation_percentage = completation_percentage;
     101        2230 :     if (currentAction.size() == 0) {
     102        1392 :         feedback.action_name_current = actionControlMsg.action_name;
     103             :     } else {
     104         838 :         feedback.action_name_current = currentAction;
     105             :     }
     106             :     
     107        2230 :     _actionServer.publishFeedback(feedback);
     108             :     
     109        2230 : }
     110             : 
     111          33 : void ROSEE::RosActionServer::sendComplete ()  {
     112             :     
     113          33 :     ROS_INFO_STREAM("ROSACTION SERVER Sending final result completed ");
     114          33 :     goalInExecution = false;
     115          33 :     newGoal = false; //even if here should be already false
     116          33 :     wantedNormError = DEFAULT_ERROR_NORM;
     117             : 
     118          66 :     rosee_msg::ROSEECommandResult result;
     119          33 :     result.completed_action = actionControlMsg;
     120             :     
     121          33 :     _actionServer.setSucceeded ( result ); 
     122             :     //TODO reinitialize actionControlMsg here or not?
     123             : 
     124         156 : }
     125             :