From c4b79a45a11cdae84ff9745862472b04c46b80d2 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Tue, 18 Feb 2020 13:06:41 +0100
Subject: [PATCH] Added set_tolerance
---
cfg/IriJointsClient.cfg | 2 ++
config/joints_module_default.yaml | 2 +-
include/iri_joints_module/iri_joints_module.h | 18 +++++--------
launch/joints_client.launch | 6 ++---
src/iri_joints_client_alg_node.cpp | 8 ++++++
src/iri_joints_module.cpp | 26 ++++++++++++++++++-
6 files changed, 45 insertions(+), 17 deletions(-)
diff --git a/cfg/IriJointsClient.cfg b/cfg/IriJointsClient.cfg
index b2e3846..d9e85a0 100755
--- a/cfg/IriJointsClient.cfg
+++ b/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"))
diff --git a/config/joints_module_default.yaml b/config/joints_module_default.yaml
index 5936532..a7b25aa 100644
--- a/config/joints_module_default.yaml
+++ b/config/joints_module_default.yaml
@@ -1,4 +1,4 @@
-rate_hz: 1.0
+rate_hz: 10.0
joint_states_watchdog_time_s: 1.0
diff --git a/include/iri_joints_module/iri_joints_module.h b/include/iri_joints_module/iri_joints_module.h
index 0d2ad93..a5dd3d2 100644
--- a/include/iri_joints_module/iri_joints_module.h
+++ b/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);
diff --git a/launch/joints_client.launch b/launch/joints_client.launch
index f994c89..d4a0c62 100644
--- a/launch/joints_client.launch
+++ b/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>
diff --git a/src/iri_joints_client_alg_node.cpp b/src/iri_joints_client_alg_node.cpp
index f5b1ac5..dc7ce6a 100644
--- a/src/iri_joints_client_alg_node.cpp
+++ b/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();
}
diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp
index ac63b17..6695291 100644
--- a/src/iri_joints_module.cpp
+++ b/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)
--
GitLab