<commit_before>#include "robot_interface/arm_ctrl.h"

using namespace std;
using namespace geometry_msgs;
using namespace baxter_core_msgs;

ArmCtrl::ArmCtrl(string _name, string _limb, bool _use_robot, bool _use_forces, bool _use_trac_ik, bool _use_cart_ctrl) :
                 RobotInterface(_name,_limb, _use_robot, THREAD_FREQ, _use_forces, _use_trac_ik, _use_cart_ctrl),
                 Gripper(_limb, _use_robot), sub_state(""), action(""), prev_action(""), sel_object_id(-1)
{
    std::string other_limb = getLimb() == "right" ? "left" : "right";

    std::string topic = "/"+getName()+"/service_"+_limb;
    service = nh.advertiseService(topic, &ArmCtrl::serviceCb, this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    topic = "/"+getName()+"/service_"+_limb+"_to_"+other_limb;
    service_other_limb = nh.advertiseService(topic, &ArmCtrl::serviceOtherLimbCb, this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    insertAction(ACTION_HOME,    &ArmCtrl::goHome);
    insertAction(ACTION_RELEASE, &ArmCtrl::openImpl);

    nh.param<bool>("internal_recovery",  internal_recovery, true);
    ROS_INFO("[%s] Internal_recovery flag set to %s", getLimb().c_str(),
                                internal_recovery==true?"true":"false");
}

bool ArmCtrl::startThread()
{
    // This allows to call multiple threads within the same thread object.
    // As written in http://en.cppreference.com/w/cpp/thread/thread/joinable :
    //      A thread that has finished executing code, but has not yet been joined is still considered
    //      an active thread of execution and is therefore joinable.
    // So, we need to join the thread in order to spun out a new one anyways.
    if (arm_thread.joinable())    { arm_thread.join(); };

    arm_thread = std::thread(&ArmCtrl::InternalThreadEntry, this);
    return arm_thread.joinable();
}

void ArmCtrl::InternalThreadEntry()
{
    nh.param<bool>("internal_recovery",  internal_recovery, true);

    std::string a =     getAction();
    int         s = int(getState());

    setState(WORKING);

    if (not isRobotUsed())
    {
        ros::Duration(2.0).sleep();
        setState(DONE);
    }
    else if (a == ACTION_HOME || a == ACTION_RELEASE)
    {
        if (callAction(a))   setState(DONE);
    }
    else if (s == START || s == ERROR ||
             s == DONE  || s == KILLED )
    {
        if (doAction(s, a))   setState(DONE);
        else                  setState(ERROR);
    }
    else
    {
        ROS_ERROR("[%s] Invalid Action %s in state %i", getLimb().c_str(), a.c_str(), s);
    }

    if (int(getState())==WORKING)
    {
        setState(ERROR);
    }

    if (int(getState())==ERROR)
    {
        ROS_ERROR("[%s] Action %s not successful! State %s %s", getLimb().c_str(), a.c_str(),
                                          string(getState()).c_str(), getSubState().c_str());
    }

    return;
}

bool ArmCtrl::serviceOtherLimbCb(baxter_collaboration_msgs::AskFeedback::Request  &req,
                                 baxter_collaboration_msgs::AskFeedback::Response &res)
{
    res.success = false;
    res.reply   = "not implemented";
    return true;
}

bool ArmCtrl::serviceCb(baxter_collaboration_msgs::DoAction::Request  &req,
                        baxter_collaboration_msgs::DoAction::Response &res)
{
    // Let's read the requested action and object to act upon
    setSubState("");
    object_ids.clear();
    setObjectID(-1);

    string action = req.action;
    std::vector<int> object_ids;
    std::string objs_str = "";

    for (size_t i = 0; i < req.objects.size(); ++i)
    {
        object_ids.push_back(req.objects[i]);
        objs_str += toString(req.objects[i]) + ", ";
    }
    objs_str = objs_str.substr(0, objs_str.size()-2); // Remove the last ", "

    ROS_INFO("[%s] Service request received. Action: %s Objects: %s", getLimb().c_str(),
                                                      action.c_str(), objs_str.c_str());

    // Print the action or object DB if requested by the user
    if      (action == LIST_ACTIONS)
    {
        printActionDB();
        res.success  = true;
        res.response = actionDBToString();
        return true;
    }
    else if (action == LIST_OBJECTS)
    {
        printObjectDB();
        res.success  = true;
        res.response = objectDBToString();
        return true;
    }

    res.success = false;

    setAction(action);

    if (action != ACTION_HOME && action != ACTION_RELEASE && action != ACTION_HOLD &&
        action != std::string(ACTION_HOLD) + "_leg" && action != std::string(ACTION_HOLD) + "_top")
    {
        setObjectIDs(areObjectsInDB(object_ids));

        if      (object_ids.size() == 0)
        {
            res.response = OBJ_NOT_IN_DB;
            ROS_ERROR("[%s] Requested object(s) are not in the database!",
                                                       getLimb().c_str());
            return true;
        }
        else if (object_ids.size() == 1)
        {
            setObjectID(object_ids[0]);
            // ROS_INFO("I will perform action %s on object with ID %i",
            //                           action.c_str(), getObjectID());
        }
        else if (object_ids.size() >  1)
        {
            setObjectID(chooseObjectID(object_ids));
        }
    }
    else if (action == ACTION_HOLD || action == std::string(ACTION_HOLD) + "_leg" ||
                                      action == std::string(ACTION_HOLD) + "_top"   )
    {
        setObjectIDs(object_ids);
    }
<commit_msg>[arm_ctrl] Minor fix with the set of actions that were included in the ICRA work.<commit_after>#include "robot_interface/arm_ctrl.h"

using namespace std;
using namespace geometry_msgs;
using namespace baxter_core_msgs;

ArmCtrl::ArmCtrl(string _name, string _limb, bool _use_robot, bool _use_forces, bool _use_trac_ik, bool _use_cart_ctrl) :
                 RobotInterface(_name,_limb, _use_robot, THREAD_FREQ, _use_forces, _use_trac_ik, _use_cart_ctrl),
                 Gripper(_limb, _use_robot), sub_state(""), action(""), prev_action(""), sel_object_id(-1)
{
    std::string other_limb = getLimb() == "right" ? "left" : "right";

    std::string topic = "/"+getName()+"/service_"+_limb;
    service = nh.advertiseService(topic, &ArmCtrl::serviceCb, this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    topic = "/"+getName()+"/service_"+_limb+"_to_"+other_limb;
    service_other_limb = nh.advertiseService(topic, &ArmCtrl::serviceOtherLimbCb, this);
    ROS_INFO("[%s] Created service server with name  : %s", getLimb().c_str(), topic.c_str());

    insertAction(ACTION_HOME,    &ArmCtrl::goHome);
    insertAction(ACTION_RELEASE, &ArmCtrl::openImpl);

    nh.param<bool>("internal_recovery",  internal_recovery, true);
    ROS_INFO("[%s] Internal_recovery flag set to %s", getLimb().c_str(),
                                internal_recovery==true?"true":"false");
}

bool ArmCtrl::startThread()
{
    // This allows to call multiple threads within the same thread object.
    // As written in http://en.cppreference.com/w/cpp/thread/thread/joinable :
    //      A thread that has finished executing code, but has not yet been joined is still considered
    //      an active thread of execution and is therefore joinable.
    // So, we need to join the thread in order to spun out a new one anyways.
    if (arm_thread.joinable())    { arm_thread.join(); };

    arm_thread = std::thread(&ArmCtrl::InternalThreadEntry, this);
    return arm_thread.joinable();
}

void ArmCtrl::InternalThreadEntry()
{
    nh.param<bool>("internal_recovery",  internal_recovery, true);

    std::string a =     getAction();
    int         s = int(getState());

    setState(WORKING);

    if (not isRobotUsed())
    {
        ros::Duration(2.0).sleep();
        setState(DONE);
    }
    else if (a == ACTION_HOME || a == ACTION_RELEASE)
    {
        if (callAction(a))   setState(DONE);
    }
    else if (s == START || s == ERROR ||
             s == DONE  || s == KILLED )
    {
        if (doAction(s, a))   setState(DONE);
        else                  setState(ERROR);
    }
    else
    {
        ROS_ERROR("[%s] Invalid Action %s in state %i", getLimb().c_str(), a.c_str(), s);
    }

    if (int(getState())==WORKING)
    {
        setState(ERROR);
    }

    if (int(getState())==ERROR)
    {
        ROS_ERROR("[%s] Action %s not successful! State %s %s", getLimb().c_str(), a.c_str(),
                                          string(getState()).c_str(), getSubState().c_str());
    }

    return;
}

bool ArmCtrl::serviceOtherLimbCb(baxter_collaboration_msgs::AskFeedback::Request  &req,
                                 baxter_collaboration_msgs::AskFeedback::Response &res)
{
    res.success = false;
    res.reply   = "not implemented";
    return true;
}

bool ArmCtrl::serviceCb(baxter_collaboration_msgs::DoAction::Request  &req,
                        baxter_collaboration_msgs::DoAction::Response &res)
{
    // Let's read the requested action and object to act upon
    setSubState("");
    object_ids.clear();
    setObjectID(-1);

    string action = req.action;
    std::vector<int> object_ids;
    std::string objs_str = "";

    for (size_t i = 0; i < req.objects.size(); ++i)
    {
        object_ids.push_back(req.objects[i]);
        objs_str += toString(req.objects[i]) + ", ";
    }
    objs_str = objs_str.substr(0, objs_str.size()-2); // Remove the last ", "

    ROS_INFO("[%s] Service request received. Action: %s Objects: %s", getLimb().c_str(),
                                                      action.c_str(), objs_str.c_str());

    // Print the action or object DB if requested by the user
    if      (action == LIST_ACTIONS)
    {
        printActionDB();
        res.success  = true;
        res.response = actionDBToString();
        return true;
    }
    else if (action == LIST_OBJECTS)
    {
        printObjectDB();
        res.success  = true;
        res.response = objectDBToString();
        return true;
    }

    res.success = false;

    setAction(action);

    if (action != ACTION_HOME && action != ACTION_RELEASE && action != ACTION_HOLD &&
        action != std::string(ACTION_HOLD) +   "_leg" && action != std::string(ACTION_HOLD) + "_top" &&
        action != "start_" + std::string(ACTION_HOLD) && action != "end_" + std::string(ACTION_HOLD))
    {
        setObjectIDs(areObjectsInDB(object_ids));

        if      (object_ids.size() == 0)
        {
            res.response = OBJ_NOT_IN_DB;
            ROS_ERROR("[%s] Requested object(s) are not in the database! Action %s",
                                                 getLimb().c_str(), action.c_str());
            return true;
        }
        else if (object_ids.size() == 1)
        {
            setObjectID(object_ids[0]);
            // ROS_INFO("I will perform action %s on object with ID %i",
            //                           action.c_str(), getObjectID());
        }
        else if (object_ids.size() >  1)
        {
            setObjectID(chooseObjectID(object_ids));
        }
    }
    else if (action == ACTION_HOLD || action == std::string(ACTION_HOLD) + "_leg" ||
                                      action == std::string(ACTION_HOLD) + "_top"   )
    {
        setObjectIDs(object_ids);
    }

{'fraction_non_alphanumeric': 0.1401017008591969, 'fraction_numerical': 0.0019288093985621604, 'mean_word_length': 2.4132256134051464, 'pattern_counts': {'":': 2, '<': 11, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 20, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4624235', 'n_tokens_mistral': 3678, 'n_tokens_neox': 3594, 'n_words': 795}