final version with back to origin position

iri_trick_or_treat/CMakeLists.txt

@@ -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} iri_common_drivers_msgs_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 <iri_common_drivers_msgs/QueryJointsMovement.h>
 #include <std_srvs/SetBool.h>
 
 // [action server client headers]
@@ -76,6 +77,9 @@ private:
 
 
   // [client attributes]
+    ros::ServiceClient joints_move_client_;
+    iri_common_drivers_msgs::QueryJointsMovement joints_move_srv_;
+
     ros::ServiceClient treat_trick_client_;
     std_srvs::SetBool treat_trick_srv_;
 
@@ -103,7 +107,9 @@ private:
   Config config_;
 
   // My variables
-  float emotions[6];
+  float JOINTS_INITIAL_POSITION[7] = {0.002045307822196906, 0.11688302408455711, -0.07818274385664722, 2.2406361185191135, 0.08482499021963152, 0.38785764141743473, -0.06385750472540101};
+
+    float emotions[6];
   int selected_present;
   // 3D position
   struct point_XYZ { // or point_RPY
@@ -169,6 +175,10 @@ private:
 
     double* getBlockCentroid(float board_width, float board_height, int board_rows, int board_cols, int num_blocks, int position_on_the_board);
 
+
+    void backToInitialPosition();
+
+
 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.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">
+	<node name="iri_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 -->

iri_trick_or_treat/src/trick_or_treat_alg_node.cpp

@@ -33,6 +33,14 @@ TrickOrTreatAlgNode::TrickOrTreatAlgNode(void)
     : algorithm_base::IriBaseAlgorithm<TrickOrTreatAlgorithm>(),
       pick_or_place_client_("/iri_wam_generic_pickorplace/pick_or_place",
                             true) {
+  //Initialize initial position for the robot
+  joints_move_srv_.request.positions.push_back(this->JOINTS_INITIAL_POSITION[0]);
+  joints_move_srv_.request.positions.push_back(this->JOINTS_INITIAL_POSITION[1]);
+  joints_move_srv_.request.positions.push_back(this->JOINTS_INITIAL_POSITION[2]);
+  joints_move_srv_.request.positions.push_back(this->JOINTS_INITIAL_POSITION[3]);
+  joints_move_srv_.request.positions.push_back(this->JOINTS_INITIAL_POSITION[4]);
+  joints_move_srv_.request.positions.push_back(this->JOINTS_INITIAL_POSITION[5]);
+  joints_move_srv_.request.positions.push_back(this->JOINTS_INITIAL_POSITION[6]);
   // init class attributes if necessary
   // this->loop_rate_ = 2;//in [Hz]
   // parameters setting for the physical board
@@ -60,6 +68,8 @@ TrickOrTreatAlgNode::TrickOrTreatAlgNode(void)
 
 
   // [init clients]
+  this->joints_move_client_ = this->public_node_handle_.serviceClient<iri_common_drivers_msgs::QueryJointsMovement>("joints_move");
+
   treat_trick_client_ = this->public_node_handle_.serviceClient<std_srvs::SetBool>("treat_trick");
 
 
@@ -80,6 +90,18 @@ void TrickOrTreatAlgNode::mainNodeThread(void) {
   // this->speech_String_msg_.data = my_var;
 
   // [fill srv structure and make request to the server]
+  ///iri_wam/iri_wam_controller/joints_move_srv_.request.data = my_var;
+  //ROS_INFO("TrickOrTreatAlgNode:: Sending New Request!");
+  //if (/iri_wam/iri_wam_controller/joints_move_client_.call(/iri_wam/iri_wam_controller/joints_move_srv_))
+  //{
+    //ROS_INFO("TrickOrTreatAlgNode:: Response: %s", /iri_wam/iri_wam_controller/joints_move_srv_.response.result);
+  //}
+  //else
+  //{
+    //ROS_INFO("TrickOrTreatAlgNode:: Failed to Call Server on topic /iri_wam/iri_wam_controller/joints_move ");
+  //}
+
+
   //treat_trick_srv_.request.data = my_var;
   //ROS_INFO("TrickOrTreatAlgNode:: Sending New Request!");
   //if (treat_trick_client_.call(treat_trick_srv_))
@@ -183,6 +205,7 @@ bool TrickOrTreatAlgNode::trick_or_treatCallback(std_srvs::SetBool::Request &req
   //do operations with req and output on res
   res.success = req.data;
 
+  ROS_INFO_STREAM("GET MESSAGE "<<res.success);
   // get the detected emotion
   switch (res.success) {
     case 0: {
@@ -328,6 +351,7 @@ bool TrickOrTreatAlgNode::trick_or_treatCallback(std_srvs::SetBool::Request &req
 
   }
 
+  backToInitialPosition();
 
   //unlock previously blocked shared variables
   //this->trick_or_treat_mutex_exit();
@@ -426,6 +450,37 @@ void TrickOrTreatAlgNode::addNodeDiagnostics(void) {}
 /*My functions*/
 
 
+
+void TrickOrTreatAlgNode::backToInitialPosition()
+{
+  ROS_DEBUG("COME BACK TO THE ORIGINAL POSITION");
+  // [fill srv structure and make request to the server]
+  ///iri_wam/iri_wam_controller/joints_move_srv_.request.data = my_var;
+  //ROS_INFO("TrickOrTreatAlgNode:: Sending New Request!");
+  //if (/iri_wam/iri_wam_controller/joints_move_client_.call(/iri_wam/iri_wam_controller/joints_move_srv_))
+  //{
+    //ROS_INFO("TrickOrTreatAlgNode:: Response: %s", /iri_wam/iri_wam_controller/joints_move_srv_.response.result);
+  //}
+  //else
+  //{
+    //ROS_INFO("TrickOrTreatAlgNode:: Failed to Call Server on topic /iri_wam/iri_wam_controller/joints_move ");
+  //}
+
+
+
+  ROS_DEBUG("InteractionSystemAlgNode:: Sending New Request!");
+  if (joints_move_client_.call(joints_move_srv_))
+  {
+    ROS_INFO_STREAM("InteractionSystemAlgNode:: Response:"<<joints_move_srv_.response.success);
+  }
+  else
+  {
+    ROS_DEBUG("InteractionSystemAlgNode:: Failed to Call Server on topicjoints_move ");
+  }
+}
+
+
+
 void TrickOrTreatAlgNode::tfTransformation2BoardFrame(
                                                            geometry_msgs::PoseStamped camera_frame,
                                                            geometry_msgs::PoseStamped iri_wam_link_base_frame,