Adding addition calls

iri_trick_or_treat/CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm iri_common_drivers_msgs actionlib iri_wam_generic_pickorplace actionlib std_msgs tf)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_srvs iri_common_drivers_msgs actionlib iri_wam_generic_pickorplace actionlib std_msgs tf)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -60,7 +60,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm iri_wam_generic_pickorplace actionlib std_msgs actionlib iri_wam_common_msgs geometry_msgs 
+ CATKIN_DEPENDS iri_base_algorithm std_srvs iri_wam_generic_pickorplace actionlib std_msgs actionlib iri_wam_common_msgs geometry_msgs 
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -94,6 +94,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} std_srvs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} iri_wam_generic_pickorplace_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
 # ******************************************************************** 

iri_trick_or_treat/include/trick_or_treat_alg_node.h

@@ -35,6 +35,7 @@
 #include <std_msgs/String.h>
 
 // [service client headers]
+#include <std_srvs/SetBool.h>
 
 // [action server client headers]
 #include <actionlib/client/simple_action_client.h>
@@ -67,8 +68,17 @@ private:
   void emotions_mutex_exit(void);
 
   // [service attributes]
+    ros::ServiceServer trick_or_treat_server_;
+    bool trick_or_treatCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+    pthread_mutex_t trick_or_treat_mutex_;
+    void trick_or_treat_mutex_enter(void);
+    void trick_or_treat_mutex_exit(void);
+
 
   // [client attributes]
+    ros::ServiceClient treat_trick_client_;
+    std_srvs::SetBool treat_trick_srv_;
+
 
   // [action server attributes]
 
@@ -94,7 +104,7 @@ private:
 
   // My variables
   float emotions[6];
-  int selected_emotion;
+  int selected_present;
   // 3D position
   struct point_XYZ { // or point_RPY
     float X;         // or R
@@ -144,10 +154,21 @@ private:
       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);
+
+
+  void tfTransformation2BoardFrame(geometry_msgs::PoseStamped board_frame,
+                                                          geometry_msgs::PoseStamped iri_wam_link_base_frame,
+                                                          double x_point,
+                                                          double y_point,
+                                                          double z_point
+                                                          );
   double *getBlockCentroidUserSide(float board_width, float board_height,
                                    int board_rows, int board_cols,
                                    int num_blocks, int position_on_the_board);
 
+
+    double* getBlockCentroid(float board_width, float board_height, int board_rows, int board_cols, int num_blocks, int position_on_the_board);
+
 public:
   /**
    * \brief Constructor

iri_trick_or_treat/launch/trick_or_treat.launch

 <launch>
 	<node pkg="tf" type="static_transform_publisher" name="wam_movements" args="0.41 -0.265 -0.0 0.0 0.0 0.0 iri_wam_link_base board_game 100" />
-	<node pkg="tf" type="static_transform_publisher" name="camera_calibration" args="-1.0 0.0 0 0 0 0 iri_wam_link_base openni_depth_frame 100" />
+	<node pkg="tf" type="static_transform_publisher" name="camera_calibration" args="-1.7 -0.2 0.2 0 0 0 board_game openni_depth_frame 100" />
 	<node name="trick_or_treat" pkg="iri_trick_or_treat" type="iri_trick_or_treat" output="screen">
 		<!--<remap from="/iri_wam_generic_pickorplace/pick_or_place" to="pick_or_place"/> -->
 

iri_trick_or_treat/package.xml

@@ -41,6 +41,7 @@
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>iri_wam_generic_pickorplace</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>std_msgs</build_depend>
@@ -49,6 +50,7 @@
   <build_depend>iri_common_drivers_msgs</build_depend>
 
   <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>std_srvs</run_depend>
   <run_depend>iri_wam_generic_pickorplace</run_depend>
   <run_depend>actionlib</run_depend>
   <run_depend>std_msgs</run_depend>