Added cancel_point_to function

cfg/IriJointsClient.cfg

@@ -48,6 +48,7 @@ point_to.add("vel_max",          double_t,  0,                               "Po
 point_to.add("send_point_to",    bool_t,    0,                               "Call point_to",         False)
 point_to.add("ik_tol",        	 double_t,  0,                               "Inverse kinematics solver tolerance",      0.001,      0.0, 1.0)
 point_to.add("send_ik_tol",      bool_t,    0,                               "Send inverse kinematic tolerance",         False)
+point_to.add("cancel",           bool_t,    0,                               "cancel point_to goal",         False)
 
 
 exit(gen.generate(PACKAGE, "IriJointsClientAlgorithm", "IriJointsClient"))

include/iri_joints_module/iri_joints_module.h

@@ -90,6 +90,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
       *
       */
     bool new_point_goal;
+    bool cancel_poin_to;
 
     // dynamic reconfigure
     /**
@@ -162,6 +163,12 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
       */
     void point_to(double x, double y, double z, double max_vel);
 
+    /**
+      * \brief Function to cancel point_to action.
+      *
+      */
+    void cancel_point_to(void);
+
     /**
       * \brief Function to change the inverse kinematics solver tolerance
       *

src/iri_joints_client_alg_node.cpp

@@ -49,16 +49,22 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
   if (config.send_point_to)
   {
     config.send_point_to = false;
-    joints_module.point_to(config.point_x, config.point_y, config.point_z, config.vel_max);
+    this->joints_module.point_to(config.point_x, config.point_y, config.point_z, config.vel_max);
   }
   if (config.send_ik_tol)
   {
     config.send_ik_tol = false;
-    if (joints_module.set_parameter("max_ik_tol", config.ik_tol))
+    if (this->joints_module.set_parameter("max_ik_tol", config.ik_tol))
       ROS_DEBUG("Tolerance set");
     else
       ROS_ERROR("Error while setting tolerance");
   }
+  if(config.cancel)
+  {
+    config.cancel = false;
+    ROS_DEBUG("Cancel action point_to");
+    this->joints_module.cancel_point_to();
+  }
   this->alg_.unlock();
 }
 

src/iri_joints_module.cpp

@@ -113,6 +113,11 @@ void CIriJointsModule::state_machine(void)
           break;
         }
       }
+      if (this->cancel_poin_to)
+      {
+        this->cancel_poin_to = false;
+        this->point_to_action.cancel();
+      }
     break;
   }
 }
@@ -190,6 +195,12 @@ void CIriJointsModule::point_to(double x, double y, double z, double max_vel)
   return;
 }
 
+void CIriJointsModule::cancel_point_to(void)
+{
+  if(this->state != IRI_JOINTS_MODULE_IDLE)
+    this->cancel_poin_to = true;
+}
+
 bool CIriJointsModule::set_tolerance(double tolerance)
 {
   this->lock();