added launch file

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 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"/> -->
+
+		<!-- This parameter defines the status of the baord after each interaction with the patient -->
+		<rosparam param="board_blocks_index">[1,2,3,4,5]</rosparam>
+		<!-- definition of the position of the board frame respect to the iri_wam_link_base -->
+		<rosparam param="board_width">0.5</rosparam>
+		<rosparam param="board_height">0.4</rosparam>
+		<rosparam param="board_rows">4</rosparam>
+		<rosparam param="board_cols">5</rosparam>
+		<rosparam param="n_cells">10</rosparam>
+	</node>
+</launch> 

iri_trick_or_treat/src/trick_or_treat_alg_node.cpp

@@ -2,11 +2,16 @@
 
 TrickOrTreatAlgNode::TrickOrTreatAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<TrickOrTreatAlgorithm>(),
-  pick_or_place_client_("pick_or_place", true)
+  pick_or_place_client_("/iri_wam_generic_pickorplace/pick_or_place", true)
 {
   //init class attributes if necessary
   //this->loop_rate_ = 2;//in [Hz]
-
+//parameters setting for the physical board
+  this->public_node_handle_.getParam("/trick_or_treat/board_width", this->board_width);
+  this->public_node_handle_.getParam("/trick_or_treat/board_height", this->board_height);
+  this->public_node_handle_.getParam("/trick_or_treat/board_rows", this->board_rows);
+  this->public_node_handle_.getParam("/trick_or_treat/board_cols", this->board_cols);
+  this->public_node_handle_.getParam("/trick_or_treat/n_cells", this->n_cells);
   // [init publishers]
   this->speech_publisher_ = this->public_node_handle_.advertise<std_msgs::String>("speech", 100);
   
@@ -69,6 +74,9 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
   this->speech_String_msg_.data = "Hello, based on your emotion I will give you a gift";
   this->speech_publisher_.publish(this->speech_String_msg_);
   ros::Duration(2.0).sleep();
+  selected_emotion = 0;
+
+
 
   //get the detected emotion
   switch (selected_emotion)
@@ -76,8 +84,8 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
     case 0:
     {
       //initialize the pick location
-      int pick_location = 0;
-      int block_id = 0;
+      int pick_location = 1;
+      int place_location = 8;
 
       geometry_msgs::PoseStamped board_frame;
       geometry_msgs::PoseStamped iri_wam_link_base_frame;
@@ -88,6 +96,7 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
       double place_grasp_block_z_ax = 0.2;
       double place_pregrasp_block_z_ax = 0.1;
       double place_postgrasp_block_z_ax = 0.25;
+      ROS_INFO("1");
 
       tfTransformation2BoardFrame(
               pick_location,
@@ -96,15 +105,17 @@ void TrickOrTreatAlgNode::mainNodeThread(void)
               pick_grasp_block_z_ax,
               pick_pregrasp_block_z_ax,
               pick_postgrasp_block_z_ax);
+      ROS_INFO("2");
       pickOrPlaceBlock(pregrasp_point, grasp_point, postgrasp_point, ini_grasp_EF_rpy, end_grasp_EF_rpy, 1, true);
+      ROS_INFO("3");
       tfTransformation2BoardFrame(
-              block_id,
+              place_location,
               board_frame,
               iri_wam_link_base_frame,
               place_grasp_block_z_ax,
               place_pregrasp_block_z_ax,
               place_postgrasp_block_z_ax);
-
+      ROS_INFO("4");
 
       //wait for the action to return
       bool finished_before_timeout = pick_or_place_client_.waitForResult(ros::Duration(50.0));
@@ -316,8 +327,9 @@ void TrickOrTreatAlgNode::tfTransformation2BoardFrame(int cell_number,
                                                            double pregrasp_off_to_block_z_ax,
                                                            double postgrasp_off_to_block_z_ax)
 {
+  ROS_INFO("5");
   board_centroids = getBlockCentroidUserSide( board_width, board_height, board_rows, board_cols, n_cells, cell_number);
-  ROS_DEBUG("coords transformation of centroid: %0.2f,%0.2f, %0.2f, %0.2f,%0.2f, %0.2f, %0.2f, for cell %i", board_centroids[0], board_centroids[1],
+  ROS_INFO("coords transformation of centroid: %0.2f,%0.2f, %0.2f, %0.2f,%0.2f, %0.2f, %0.2f, for cell %i", board_centroids[0], board_centroids[1],
             board_centroids[2], board_centroids[3], board_centroids[4], board_centroids[5], board_centroids[6], cell_number);
 
   board_frame.header.frame_id = "board_game";
@@ -329,11 +341,11 @@ void TrickOrTreatAlgNode::tfTransformation2BoardFrame(int cell_number,
   board_frame.pose.orientation.z = board_centroids[5];
   board_frame.pose.orientation.w = board_centroids[6];
 
-  ROS_DEBUG("Before pose transform");
+  ROS_INFO("Before pose transform");
   listener.waitForTransform("iri_wam_link_base", "board_game", ros::Time::now(), ros::Duration(15.0));
   listener.transformPose("iri_wam_link_base", board_frame, iri_wam_link_base_frame);
 
-  ROS_DEBUG("transform pose of centroid: %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f ",
+  ROS_INFO("transform pose of centroid: %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f, %0.2f ",
             iri_wam_link_base_frame.pose.position.x, iri_wam_link_base_frame.pose.position.y, iri_wam_link_base_frame.pose.position.z,
             iri_wam_link_base_frame.pose.orientation.x, iri_wam_link_base_frame.pose.orientation.y, iri_wam_link_base_frame.pose.orientation.z, iri_wam_link_base_frame.pose.orientation.w);