Changed the service to request actions. Now it allows for an array of…

… objects to act upon. The final object will be randomly chosen between those available. Modified [arm_ctrl], [cartesianEstimatorClient], [artag_ctrl], [toolPicker], [ARucoClient] to take advantage of the change accordingly. Cc @omangin , @sarah-widder, @brahmgardner

lib/include/flatpack_furniture/artag_ctrl.h

@@ -1,6 +1,9 @@
 #ifndef __ARTAG_CTRL_H__
 #define __ARTAG_CTRL_H__
 
+#include <stdlib.h>
+#include <time.h>
+
 #include <aruco_msgs/MarkerArray.h>
 
 #include "robot_interface/arm_ctrl.h"
@@ -87,6 +90,18 @@ class ARTagCtrl : public ArmCtrl, public ARucoClient
      */
     geometry_msgs::Quaternion computeHOorientation();
 
+protected:
+    /**
+     * Chooses the object to act upon according to some rule. This method
+     * needs to be specialized in any derived class because it is dependent
+     * on the type of action and the type of sensory capabilities available.
+     *
+     * @param _objs The list of IDs of objects to choose from
+     * @return      the ID of the chosen object (by default the ID of the
+     *              first object will be chosen)
+     */
+    int chooseObjectID(std::vector<int> _objs);
+
 public:
     /**
      * Constructor

lib/include/robot_interface/arm_ctrl.h

@@ -190,24 +190,42 @@ class ArmCtrl : public RobotInterface, public Gripper, public ROSThread
     bool removeObject(int id);
 
     /**
-     * Gets an object from the object database
+     * Gets an object's name from the object database
      *
-     * @param    id the requested object
+     * @param    id the requested object's ID
      * @return      the associated string
      *              (empty string if object is not there)
      */
-    std::string getObjectName(int id);
+    std::string getObjectNameFromDB(int id);
+
+    /**
+     * Gets an object's ID from the object database
+     *
+     * @param   _name the requested object's name
+     * @return      the associated id
+     *              (-1 if object is not there)
+     */
+    int getObjectIDFromDB(std::string _name);
 
     /**
      * Checks if an object is available in the database
-     * @param  id the action to check for
-     * @param  insertAction flag to know if the method has been called
-     *                      inside insertAction (it only removes the
-     *                      ROS_ERROR if the action is not in the DB)
-     * @return   true/false if the action is available in the database
+     *
+     * @param  id the object to check for
+     * @return    true/false if the object is available in the database
      */
     bool isObjectInDB(int id);
 
+    /**
+     * Chooses the object to act upon according to some rule. This method
+     * needs to be specialized in any derived class because it is dependent
+     * on the type of action and the type of sensory capabilities available.
+     *
+     * @param _objs The list of IDs of objects to choose from
+     * @return      the ID of the chosen object (by default the ID of the
+     *              first object will be chosen)
+     */
+    virtual int chooseObjectID(std::vector<int> _objs) { return _objs[0]; };
+
     /**
      * Prints the object database to screen.
      */
@@ -317,16 +335,16 @@ class ArmCtrl : public RobotInterface, public Gripper, public ROSThread
 
     /* Self-explaining "setters" */
     void setSubState(std::string _state) { sub_state =  _state; };
-    virtual void setObjectID(int _obj)   { object_id =    _obj; };
+    virtual void   setObjectID(int _obj) { object_id =    _obj; };
     void setAction(std::string _action);
 
     void setState(int _state);
 
     /* Self-explaining "getters" */
-    std::string getSubState() { return sub_state; };
-    std::string getAction()   { return    action; };
-    int         getObjectID() { return object_id; };
-    bool        getInternalRecovery() { return internal_recovery; };
+    std::string  getSubState() { return         sub_state; };
+    std::string    getAction() { return            action; };
+    int          getObjectID() { return         object_id; };
+    bool getInternalRecovery() { return internal_recovery; };
 };
 
 #endif

lib/include/robot_perception/aruco_client.h

@@ -16,12 +16,19 @@ class ARucoClient
     std::string _limb; // Limb of the gripper: left or right
 
     // Subscriber to the ARuco detector,
-    // plus some flags related to it
     ros::Subscriber _aruco_sub;
-    bool              aruco_ok;
-    bool          marker_found;
 
-    int          marker_id;
+    // List of available markers
+    std::vector<int> available_markers;
+
+    // Bool to check if ARuco is fine or not
+    bool     aruco_ok;
+
+    // Bool to check if the marker was found or not
+    bool marker_found;
+
+    // ID of the marker to detect
+    int     marker_id;
 
     // Marker position and orientation
     geometry_msgs::Point        _curr_marker_pos;
@@ -40,6 +47,12 @@ class ARucoClient
      */
     void ARucoCb(const aruco_msgs::MarkerArray& msg);
 
+    /**
+     * Waits to get feedback from aruco
+     * @return true/false if success/failure
+     */
+    bool waitForARucoOK();
+
     /**
      * Waits for useful data coming from ARuco
      * @return true/false if success/failure
@@ -51,16 +64,28 @@ class ARucoClient
     */
     bool is_aruco_ok() { return aruco_ok; };
 
-    /* Self-explaining "setters" */
+    /* SETTERS */
     void setMarkerID(int _id)            { marker_id =     _id; };
 
-    /* Self-explaining "getters" */
+    /* GETTERS */
     geometry_msgs::Point      getMarkerPos() { return _curr_marker_pos; };
     geometry_msgs::Quaternion getMarkerOri() { return _curr_marker_ori; };
 
     std::string getArucoLimb() { return     _limb; };
     int         getMarkerID()  { return marker_id; };
 
+    /**
+     * Returns a list of available markers
+     * @return a list of available markers
+     */
+    std::vector<int> getAvailableMarkers() { return available_markers; };
+
+    /**
+     * Looks if a set of markers is present among those available.
+     * @return the subset of available markers among those avilable
+     */
+    std::vector<int> getAvailableMarkers(std::vector<int> _markers);
+
 public:
     ARucoClient(std::string name, std::string limb);
     ~ARucoClient();

lib/include/robot_perception/cartesian_estimator_client.h

@@ -15,13 +15,20 @@ class CartesianEstimatorClient
 
     std::string    limb; // Limb of the gripper: left or right
 
-    // Subscriber to the CartesianEstimator detector,
-    // plus some flags related to it
+    // Subscriber to the CartesianEstimator detector
     ros::Subscriber  cartest_sub;
-    bool              cartest_ok;
-    bool            object_found;
 
-    std::string      object_name;
+    // List of available objects
+    std::vector<std::string> available_objects;
+
+    // Bool to check the CartesianEstimator is fine or not
+    bool         cartest_ok;
+
+    // Bool to check if the object was found or not
+    bool       object_found;
+
+    // Name of the object to detect
+    std::string object_name;
 
     // Object position and orientation
     geometry_msgs::Point        curr_object_pos;
@@ -40,6 +47,12 @@ class CartesianEstimatorClient
      */
     void ObjectCb(const baxter_collaboration::ObjectsArray& _msg);
 
+    /**
+     * Waits to get feedback from CartesianEstimator
+     * @return true/false if success/failure
+     */
+    bool waitForCartEstOK();
+
     /**
      * Waits for useful data coming from CartesianEstimator
      * @return true/false if success/failure
@@ -51,16 +64,28 @@ class CartesianEstimatorClient
     */
     bool is_cartesian_estimator_ok() { return cartest_ok; };
 
-    /* Self-explaining "setters" */
+    /* SETTERS */
     void setObjectName(std::string _name)    { object_name =     _name; };
 
-    /* Self-explaining "getters" */
+    /* GETTERS */
     geometry_msgs::Point      getObjectPos() { return curr_object_pos; };
     geometry_msgs::Quaternion getObjectOri() { return curr_object_ori; };
 
     std::string getCartesianEstimatorLimb() { return        limb; };
     std::string getObjectName()             { return object_name; };
 
+    /**
+     * Returns a list of available markers
+     * @return a list of available markers
+     */
+    std::vector<std::string> getAvailableObjects() { return available_objects; };
+
+    /**
+     * Looks if a set of markers is present among those available.
+     * @return the subset of available markers among those avilable
+     */
+    std::vector<std::string> getAvailableObjects(std::vector<std::string> _objects);
+
 public:
     CartesianEstimatorClient(std::string _name, std::string _limb);
     ~CartesianEstimatorClient();

lib/src/flatpack_furniture/artag_ctrl.cpp

@@ -263,6 +263,23 @@ bool ARTagCtrl::pickARTag()
     return false;
 }
 
+int ARTagCtrl::chooseObjectID(std::vector<int> _objs)
+{
+    int res = -1;
+
+    if (!hoverAbovePool())      return res;
+    if (!waitForARucoOK())      return res;
+
+    std::vector<int> av_markers = getAvailableMarkers(_objs);
+
+    if (av_markers.size() == 0) return res;
+
+    std::srand(std::time(0)); //use current time as seed
+    res = av_markers[rand() % av_markers.size()];
+
+    return res;
+}
+
 geometry_msgs::Quaternion ARTagCtrl::computeHOorientation()
 {
     // Get the rotation matrix for the marker, as retrieved from ARuco

lib/src/robot_interface/arm_ctrl.cpp

@@ -7,7 +7,7 @@ using namespace baxter_core_msgs;
 
 ArmCtrl::ArmCtrl(string _name, string _limb, bool _no_robot, bool _use_forces, bool _use_trac_ik, bool _use_cart_ctrl) :
                  RobotInterface(_name,_limb, _no_robot, _use_forces, _use_trac_ik, _use_cart_ctrl),
-                 Gripper(_limb, _no_robot), sub_state(""), action(""), object_id(-1)
+                 Gripper(_limb, _no_robot), sub_state(""), action("")
 {
     std::string topic = "/"+getName()+"/state_"+_limb;
     state_pub = _n.advertise<baxter_collaboration::ArmState>(topic,1);
@@ -91,10 +91,18 @@ bool ArmCtrl::serviceCb(baxter_collaboration::DoAction::Request  &req,
                         baxter_collaboration::DoAction::Response &res)
 {
     string action = req.action;
-    int    obj    = req.object;
+    std::vector<int> objs;
+    std::string objs_str = "";
 
-    ROS_INFO("[%s] Service request received. Action: %s object: %i", getLimb().c_str(),
-                                                                   action.c_str(), obj);
+    for (size_t i = 0; i < req.object.size(); ++i)
+    {
+        objs.push_back(req.object[i]);
+        objs_str += intToString(req.object[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());
 
     if (action == PROT_ACTION_LIST)
     {
@@ -107,7 +115,16 @@ bool ArmCtrl::serviceCb(baxter_collaboration::DoAction::Request  &req,
     res.success = false;
 
     setAction(action);
-    setObjectID(obj);
+    if      (objs.size() == 1)
+    {
+        setObjectID(objs[0]);
+    }
+    else if (objs.size() >  1)
+    {
+        setObjectID(chooseObjectID(objs));
+    }
+
+    ROS_INFO("I will pick up object with ID %i", getObjectID());
 
     startInternalThread();
     ros::Duration(0.5).sleep();
@@ -186,7 +203,7 @@ bool ArmCtrl::removeObject(int id)
     return false;
 }
 
-string ArmCtrl::getObjectName(int id)
+string ArmCtrl::getObjectNameFromDB(int id)
 {
     if (isObjectInDB(id))
     {
@@ -196,6 +213,15 @@ string ArmCtrl::getObjectName(int id)
     return "";
 }
 
+int ArmCtrl::getObjectIDFromDB(string _name)
+{
+    for( map<int, string>::const_iterator it = object_db.begin(); it != object_db.end(); ++it )
+    {
+        if (_name == it->second) return it->first;
+    }
+    return -1;
+}
+
 bool ArmCtrl::isObjectInDB(int id)
 {
     if (object_db.find(id) != object_db.end()) return true;
@@ -496,7 +522,7 @@ void ArmCtrl::publishState()
 
     msg.state  = string(getState());
     msg.action = getAction();
-    msg.object = getObjectName(getObjectID());
+    msg.object = getObjectNameFromDB(getObjectID());
 
     state_pub.publish(msg);
 }