Get hand position

iri_trick_or_treat/include/trick_or_treat_alg_node.h

-// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author 
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial,
+// CSIC-UPC.
+// Author
 // All rights reserved.
 //
 // This file is part of iri-ros-pkg
@@ -15,9 +16,10 @@
 //
 // You should have received a copy of the GNU Lesser General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
-// 
-// IMPORTANT NOTE: This code has been generated through a script from the 
-// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the
+// correctness
 // of the scripts. ROS topics can be easly add by using those scripts. Please
 // refer to the IRI wiki page for more information:
 // http://wikiri.upc.es/index.php/Robotics_Lab
@@ -25,8 +27,8 @@
 #ifndef _trick_or_treat_alg_node_h_
 #define _trick_or_treat_alg_node_h_
 
-#include <iri_base_algorithm/iri_base_algorithm.h>
 #include "trick_or_treat_alg.h"
+#include <iri_base_algorithm/iri_base_algorithm.h>
 
 // [publisher subscriber headers]
 #include <std_msgs/Float32MultiArray.h>
@@ -39,165 +41,170 @@
 #include <actionlib/client/terminal_state.h>
 #include <iri_wam_generic_pickorplace/PickOrPlaceAction.h>
 
-#include <geometry_msgs/PoseStamped.h>
 #include <algorithm>
+#include <geometry_msgs/PoseStamped.h>
 #include <tf/tf.h>
-#include <tf/transform_listener.h>
 #include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
 /**
  * \brief IRI ROS Specific Algorithm Class
  *
  */
-class TrickOrTreatAlgNode : public algorithm_base::IriBaseAlgorithm<TrickOrTreatAlgorithm>
-{
-  private:
-    // [publisher attributes]
-    ros::Publisher speech_publisher_;
-    std_msgs::String speech_String_msg_;
-
-
-    // [subscriber attributes]
-    ros::Subscriber emotions_subscriber_;
-    void emotions_callback(const std_msgs::Float32MultiArray::ConstPtr& msg);
-    pthread_mutex_t emotions_mutex_;
-    void emotions_mutex_enter(void);
-    void emotions_mutex_exit(void);
-
-
-    // [service attributes]
-
-    // [client attributes]
-
-    // [action server attributes]
-
-    // [action client attributes]
-    actionlib::SimpleActionClient<iri_wam_generic_pickorplace::PickOrPlaceAction> pick_or_place_client_;
-    iri_wam_generic_pickorplace::PickOrPlaceGoal pick_or_place_goal_;
-    bool pick_or_placeMakeActionRequest();
-    void pick_or_placeDone(const actionlib::SimpleClientGoalState& state,  const iri_wam_generic_pickorplace::PickOrPlaceResultConstPtr& result);
-    void pick_or_placeActive();
-    void pick_or_placeFeedback(const iri_wam_generic_pickorplace::PickOrPlaceFeedbackConstPtr& feedback);
-
-
-
-    /**
-    * \brief config variable
-    *
-    * This variable has all the driver parameters defined in the cfg config file.
-    * Is updated everytime function config_update() is called.
-    */
-    Config config_;
-
-
-    //My variables
-    float emotions[6];
-    int selected_emotion;
-// 3D position
-    struct point_XYZ {	//or point_RPY
-        float X;	//or R
-        float Y;	//or P
-        float Z;	//or Y
-    };
-    int Pick_or_Place_State;
-    int Pick_or_Place_Result;
-    int Pick_or_Place_Feedback;
-//Point where the robot goes before grasping an object
-    point_XYZ pregrasp_point;
-    //Points where the robot goes when it wants to grasp an object
-    point_XYZ grasp_point;
-    //Point where the robot goes after it grasps the object
-    point_XYZ postgrasp_point;
-    //init point with the desired end effector rotations
-    point_XYZ ini_grasp_EF_rpy;
-    //end point with the desired end effector rotations
-    point_XYZ end_grasp_EF_rpy;
-    //Point where the robot goes before ungrasping an object
-    point_XYZ preungrasp_point;
-    //Points where the robot goes when it wants to ungrasp an object
-    point_XYZ ungrasp_point;
-    //Point where the robot goes after it ungrasps the object
-    point_XYZ postungrasp_point;
-    //init point with the desired end effector rotations
-    point_XYZ ini_ungrasp_EF_rpy;
-    //end point with the desired end effector rotations
-    point_XYZ end_ungrasp_EF_rpy;
-
-    float board_width;
-    float board_height;
-    float board_rows;
-    float board_cols;
-    float n_cells;
-
-    double* board_centroids;
-
-    tf::TransformListener listener;
-
-
-    //My functions
-    void pickOrPlaceBlock(point_XYZ pregrasp_point, point_XYZ grasp_point, point_XYZ postgrasp_point, point_XYZ ini_grasp_EF_rpy, point_XYZ end_grasp_EF_rpy, int state, bool pick);
-    void tfTransformation2BoardFrame(int cell_number, geometry_msgs::PoseStamped board_frame, geometry_msgs::PoseStamped iri_wam_link_base_frame, double  grasp_off_to_block_z_ax, double pregrasp_off_to_block_z_ax, double postgrasp_off_to_block_z_ax);
-    double* getBlockCentroidUserSide(float board_width, float board_height, int board_rows, int board_cols, int num_blocks, int position_on_the_board);
-
-
-
+class TrickOrTreatAlgNode
+    : public algorithm_base::IriBaseAlgorithm<TrickOrTreatAlgorithm> {
+private:
+  // [publisher attributes]
+  ros::Publisher speech_publisher_;
+  std_msgs::String speech_String_msg_;
+
+  tf::Vector3 get_hand_position(std::string);
+
+  // [subscriber attributes]
+  ros::Subscriber emotions_subscriber_;
+  void emotions_callback(const std_msgs::Float32MultiArray::ConstPtr &msg);
+  pthread_mutex_t emotions_mutex_;
+  void emotions_mutex_enter(void);
+  void emotions_mutex_exit(void);
+
+  // [service attributes]
+
+  // [client attributes]
+
+  // [action server attributes]
+
+  // [action client attributes]
+  actionlib::SimpleActionClient<iri_wam_generic_pickorplace::PickOrPlaceAction>
+      pick_or_place_client_;
+  iri_wam_generic_pickorplace::PickOrPlaceGoal pick_or_place_goal_;
+  bool pick_or_placeMakeActionRequest();
+  void pick_or_placeDone(
+      const actionlib::SimpleClientGoalState &state,
+      const iri_wam_generic_pickorplace::PickOrPlaceResultConstPtr &result);
+  void pick_or_placeActive();
+  void pick_or_placeFeedback(
+      const iri_wam_generic_pickorplace::PickOrPlaceFeedbackConstPtr &feedback);
+
+  /**
+  * \brief config variable
+  *
+  * This variable has all the driver parameters defined in the cfg config file.
+  * Is updated everytime function config_update() is called.
+  */
+  Config config_;
+
+  // My variables
+  float emotions[6];
+  int selected_emotion;
+  // 3D position
+  struct point_XYZ { // or point_RPY
+    float X;         // or R
+    float Y;         // or P
+    float Z;         // or Y
+  };
+  int Pick_or_Place_State;
+  int Pick_or_Place_Result;
+  int Pick_or_Place_Feedback;
+  // Point where the robot goes before grasping an object
+  point_XYZ pregrasp_point;
+  // Points where the robot goes when it wants to grasp an object
+  point_XYZ grasp_point;
+  // Point where the robot goes after it grasps the object
+  point_XYZ postgrasp_point;
+  // init point with the desired end effector rotations
+  point_XYZ ini_grasp_EF_rpy;
+  // end point with the desired end effector rotations
+  point_XYZ end_grasp_EF_rpy;
+  // Point where the robot goes before ungrasping an object
+  point_XYZ preungrasp_point;
+  // Points where the robot goes when it wants to ungrasp an object
+  point_XYZ ungrasp_point;
+  // Point where the robot goes after it ungrasps the object
+  point_XYZ postungrasp_point;
+  // init point with the desired end effector rotations
+  point_XYZ ini_ungrasp_EF_rpy;
+  // end point with the desired end effector rotations
+  point_XYZ end_ungrasp_EF_rpy;
+
+  float board_width;
+  float board_height;
+  float board_rows;
+  float board_cols;
+  float n_cells;
+
+  double *board_centroids;
+
+  tf::TransformListener listener;
+
+  // My functions
+  void pickOrPlaceBlock(point_XYZ pregrasp_point, point_XYZ grasp_point,
+                        point_XYZ postgrasp_point, point_XYZ ini_grasp_EF_rpy,
+                        point_XYZ end_grasp_EF_rpy, int state, bool pick);
+  void tfTransformation2BoardFrame(
+      int cell_number, geometry_msgs::PoseStamped board_frame,
+      geometry_msgs::PoseStamped iri_wam_link_base_frame,
+      double grasp_off_to_block_z_ax, double pregrasp_off_to_block_z_ax,
+      double postgrasp_off_to_block_z_ax);
+  double *getBlockCentroidUserSide(float board_width, float board_height,
+                                   int board_rows, int board_cols,
+                                   int num_blocks, int position_on_the_board);
 
 public:
-   /**
-    * \brief Constructor
-    * 
-    * This constructor initializes specific class attributes and all ROS
-    * communications variables to enable message exchange.
-    */
-    TrickOrTreatAlgNode(void);
-
-   /**
-    * \brief Destructor
-    * 
-    * This destructor frees all necessary dynamic memory allocated within this
-    * this class.
-    */
-    ~TrickOrTreatAlgNode(void);
-
-  protected:
-   /**
-    * \brief main node thread
-    *
-    * This is the main thread node function. Code written here will be executed
-    * in every node loop while the algorithm is on running state. Loop frequency 
-    * can be tuned by modifying loop_rate attribute.
-    *
-    * Here data related to the process loop or to ROS topics (mainly data structs
-    * related to the MSG and SRV files) must be updated. ROS publisher objects 
-    * must publish their data in this process. ROS client servers may also
-    * request data to the corresponding server topics.
-    */
-    void mainNodeThread(void);
-
-   /**
-    * \brief dynamic reconfigure server callback
-    * 
-    * This method is called whenever a new configuration is received through
-    * the dynamic reconfigure. The derivated generic algorithm class must 
-    * implement it.
-    *
-    * \param config an object with new configuration from all algorithm 
-    *               parameters defined in the config file.
-    * \param level  integer referring the level in which the configuration
-    *               has been changed.
-    */
-    void node_config_update(Config &config, uint32_t level);
-
-   /**
-    * \brief node add diagnostics
-    *
-    * In this abstract function additional ROS diagnostics applied to the 
-    * specific algorithms may be added.
-    */
-    void addNodeDiagnostics(void);
-
-    // [diagnostic functions]
-    
-    // [test functions]
+  /**
+   * \brief Constructor
+   *
+   * This constructor initializes specific class attributes and all ROS
+   * communications variables to enable message exchange.
+   */
+  TrickOrTreatAlgNode(void);
+
+  /**
+   * \brief Destructor
+   *
+   * This destructor frees all necessary dynamic memory allocated within this
+   * this class.
+   */
+  ~TrickOrTreatAlgNode(void);
+
+protected:
+  /**
+   * \brief main node thread
+   *
+   * This is the main thread node function. Code written here will be executed
+   * in every node loop while the algorithm is on running state. Loop frequency
+   * can be tuned by modifying loop_rate attribute.
+   *
+   * Here data related to the process loop or to ROS topics (mainly data structs
+   * related to the MSG and SRV files) must be updated. ROS publisher objects
+   * must publish their data in this process. ROS client servers may also
+   * request data to the corresponding server topics.
+   */
+  void mainNodeThread(void);
+
+  /**
+   * \brief dynamic reconfigure server callback
+   *
+   * This method is called whenever a new configuration is received through
+   * the dynamic reconfigure. The derivated generic algorithm class must
+   * implement it.
+   *
+   * \param config an object with new configuration from all algorithm
+   *               parameters defined in the config file.
+   * \param level  integer referring the level in which the configuration
+   *               has been changed.
+   */
+  void node_config_update(Config &config, uint32_t level);
+
+  /**
+   * \brief node add diagnostics
+   *
+   * In this abstract function additional ROS diagnostics applied to the
+   * specific algorithms may be added.
+   */
+  void addNodeDiagnostics(void);
+
+  // [diagnostic functions]
+
+  // [test functions]
 };
 
 #endif