Added set_tolerance

cfg/IriJointsClient.cfg

@@ -46,6 +46,8 @@ point_to.add("point_y",          double_t,  0,                               "Po
 point_to.add("point_z",          double_t,  0,                               "Point Z coordenate",                       0.0,      -100.0, 100.0)
 point_to.add("vel_max",          double_t,  0,                               "Pointing maximum velocity",                       0.5,      -0.1, 6.28)
 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)
 
 
 exit(gen.generate(PACKAGE, "IriJointsClientAlgorithm", "IriJointsClient"))

config/joints_module_default.yaml

-rate_hz: 1.0
+rate_hz: 10.0
 
 joint_states_watchdog_time_s: 1.0
 

include/iri_joints_module/iri_joints_module.h

@@ -159,20 +159,16 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
       * \param z Z coordenate on point_frame.
       *
       * \param max_vel The maximum velocity for the servos.
-      *
-      * \return a boolean indicating wheather the request has been successfull (true)
-      *         or not (false).
       */
-    bool point_to(double x, double y, double z, double max_vel);
-
-    //To maximum servo vel; Problem diferent max velocities of each servo.
-    bool point_to(double x, double y, double z);
+    void point_to(double x, double y, double z, double max_vel);
 
-    //With ik_max_error; Problem Dynamic reconfigure point_to
-    bool point_to(double x, double y, double z, double max_vel, double tolerance);
+    /**
+      * \brief Function to change the inverse kinematics solver tolerance
+      *
+      * \param tolerance Inverse kinematics tolerance.
+      */
+    bool set_tolerance(double tolerance);
 
-    //With ik_max_error; Problem Dynamic reconfigure point_to
-    //bool point_to(double x, double y, double z, double tolerance);
 
     //Change?; Problem dynamic reconfigure module, solution param out of DynRec
     void set_point_frame(std::string &point_frame);

launch/joints_client.launch

@@ -11,10 +11,8 @@
              to="/point_to/point_to"/>
     <remap from="~/joints_module/joint_states"
              to="/three_servos/joint_states"/>
-    <!--<remap from="~/joints_module/clear_costmaps"
-             to="/ana/move_base/clear_costmaps"/>
-    <remap from="~/joints_module/move_base_reconf"
-             to="/ana/move_base/DWAPlannerROS/set_parameters"/>-->
+    <remap from="~/joints_module/point_to_reconf"
+             to="/point_to/set_parameters"/>
 
     <rosparam file="$(find iri_joints_module)/config/joints_module_default.yaml" command="load" ns="joints_module" />
   </node>

src/iri_joints_client_alg_node.cpp

@@ -51,6 +51,14 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
     config.send_point_to = false;
     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_tolerance(config.ik_tol))
+      ROS_INFO("Tolerance set");
+    else
+      ROS_ERROR("Error while setting tolerance");
+  }
   this->alg_.unlock();
 }
 

src/iri_joints_module.cpp

@@ -93,6 +93,7 @@ void CIriJointsModule::state_machine(void)
       }
       if (this->new_point_goal)
       {
+        this->point_to_action.stop_timeout();
         switch (this->point_to_action.make_request(this->point_to_goal))
         {
           case ACT_SRV_SUCCESS: 
@@ -176,7 +177,7 @@ iri_joints_module_status_t CIriJointsModule::get_status(void)
   return this->status;
 }
 
-bool CIriJointsModule::point_to(double x, double y, double z, double max_vel)
+void CIriJointsModule::point_to(double x, double y, double z, double max_vel)
 {
   this->lock();
   this->point_to_goal.target.point.x = x;
@@ -186,6 +187,29 @@ bool CIriJointsModule::point_to(double x, double y, double z, double max_vel)
   this->point_to_goal.target.header.stamp = ros::Time::now();
   this->new_point_goal = true;
   this->unlock();
+  return;
+}
+
+bool CIriJointsModule::set_tolerance(double tolerance)
+{
+  this->lock();
+  if(!this->point_to_reconf.set_parameter("max_ik_tol",tolerance))
+  {
+    if(this->point_to_reconf.get_status()==DYN_RECONF_NO_CHANGE)
+    {
+      this->status = IRI_JOINTS_MODULE_SET_PARAM_FAIL;
+      this->unlock();
+      return false;
+    }
+    else// the parameter does not exist
+    {
+      this->status = IRI_JOINTS_MODULE_PARAM_NOT_PRESENT;
+      this->unlock();
+      return false;
+    }
+  }
+  this->unlock();
+  return true;
 }
 
 bool CIriJointsModule::set_parameter(const std::string &name_space,const std::string &name,bool value)