<commit_before>#include "object_manipulation_actions/actionExecutorArmToCarry.h"
#include <pluginlib/class_list_macros.h>
#include "tidyup_utils/planning_scene_interface.h"

//PLUGINLIB_DECLARE_CLASS(object_manipulation_actions, action_executor_arm_to_carry,
//        object_manipulation_actions::ActionExecutorArmToCarry,
//        continual_planning_executive::ActionExecutorInterface)
PLUGINLIB_EXPORT_CLASS(object_manipulation_actions::ActionExecutorArmToCarry, continual_planning_executive::ActionExecutorInterface)

namespace object_manipulation_actions
{

    void ActionExecutorArmToCarry::initialize(const std::deque<std::string> & arguments)
    {
        ActionExecutorActionlib<tidyup_msgs::PostGraspPositionAction, tidyup_msgs::PostGraspPositionGoal,
        tidyup_msgs::PostGraspPositionResult>::initialize(arguments);

        _armStatePredicateName = "arm-state";
        _armAtCarryConstantName = "arm_carrying";

        if(arguments.size() >= 3) {     // 3rd param: arm-state predicate name
            _armStatePredicateName = arguments[2];
        }
        if(arguments.size() >= 4) {     // 4th param: arm_at_side constant name
            _armAtCarryConstantName = arguments[3];
        }
    }

    bool ActionExecutorArmToCarry::fillGoal(tidyup_msgs::PostGraspPositionGoal & goal,
            const DurativeAction & a, const SymbolicState & current)
    {
        if(!PlanningSceneInterface::instance()->resetPlanningScene())   // FIXME try anyways?
            ROS_ERROR("%s: PlanningScene reset failed.", __PRETTY_FUNCTION__);

        goal.left_arm = false;
        goal.right_arm = false;

        ROS_ASSERT(a.parameters.size() == 1);
        string arm = a.parameters[0];
        if(arm == "left_arm") {
            goal.left_arm = true;
        }
        if(arm == "right_arm") {
            goal.right_arm = true;
        }

        return (goal.left_arm || goal.right_arm);
    }

    void ActionExecutorArmToCarry::updateState(const actionlib::SimpleClientGoalState & actionReturnState,
            const tidyup_msgs::PostGraspPositionResult & result,
            const DurativeAction & a, SymbolicState & current)
    {
        ROS_INFO("PostGraspPosition returned result: %s", result.result.c_str());
        ROS_ASSERT(a.parameters.size() == 1);
        string arm = a.parameters[0];
        if(actionReturnState == actionlib::SimpleClientGoalState::SUCCEEDED) {
            ROS_INFO("Post Grasp Position succeeded.");
            current.setObjectFluent(_armStatePredicateName, arm, _armAtCarryConstantName);
        } else {
            current.setObjectFluent(_armStatePredicateName, arm, "arm_unknown");
        }
    }

};

<commit_msg>removed obsolete planning scene interface<commit_after>#include "object_manipulation_actions/actionExecutorArmToCarry.h"
#include <pluginlib/class_list_macros.h>
//#include "tidyup_utils/planning_scene_interface.h"

//PLUGINLIB_DECLARE_CLASS(object_manipulation_actions, action_executor_arm_to_carry,
//        object_manipulation_actions::ActionExecutorArmToCarry,
//        continual_planning_executive::ActionExecutorInterface)
PLUGINLIB_EXPORT_CLASS(object_manipulation_actions::ActionExecutorArmToCarry, continual_planning_executive::ActionExecutorInterface)

namespace object_manipulation_actions
{

    void ActionExecutorArmToCarry::initialize(const std::deque<std::string> & arguments)
    {
        ActionExecutorActionlib<tidyup_msgs::PostGraspPositionAction, tidyup_msgs::PostGraspPositionGoal,
        tidyup_msgs::PostGraspPositionResult>::initialize(arguments);

        _armStatePredicateName = "arm-state";
        _armAtCarryConstantName = "arm_carrying";

        if(arguments.size() >= 3) {     // 3rd param: arm-state predicate name
            _armStatePredicateName = arguments[2];
        }
        if(arguments.size() >= 4) {     // 4th param: arm_at_side constant name
            _armAtCarryConstantName = arguments[3];
        }
    }

    bool ActionExecutorArmToCarry::fillGoal(tidyup_msgs::PostGraspPositionGoal & goal,
            const DurativeAction & a, const SymbolicState & current)
    {
//        if(!PlanningSceneInterface::instance()->resetPlanningScene())   // FIXME try anyways?
//            ROS_ERROR("%s: PlanningScene reset failed.", __PRETTY_FUNCTION__);

        goal.left_arm = false;
        goal.right_arm = false;

        ROS_ASSERT(a.parameters.size() == 1);
        string arm = a.parameters[0];
        if(arm == "left_arm") {
            goal.left_arm = true;
        }
        if(arm == "right_arm") {
            goal.right_arm = true;
        }

        return (goal.left_arm || goal.right_arm);
    }

    void ActionExecutorArmToCarry::updateState(const actionlib::SimpleClientGoalState & actionReturnState,
            const tidyup_msgs::PostGraspPositionResult & result,
            const DurativeAction & a, SymbolicState & current)
    {
        ROS_INFO("PostGraspPosition returned result: %s", result.result.c_str());
        ROS_ASSERT(a.parameters.size() == 1);
        string arm = a.parameters[0];
        if(actionReturnState == actionlib::SimpleClientGoalState::SUCCEEDED) {
            ROS_INFO("Post Grasp Position succeeded.");
            current.setObjectFluent(_armStatePredicateName, arm, _armAtCarryConstantName);
        } else {
            current.setObjectFluent(_armStatePredicateName, arm, "arm_unknown");
        }
    }

};


{'fraction_non_alphanumeric': 0.1022600963319748, 'fraction_numerical': 0.003705075954057058, 'mean_word_length': 3.4290401968826907, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 15, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5042703', 'n_tokens_mistral': 1653, 'n_tokens_neox': 1595, 'n_words': 236}