From 2341c038b58e8f3a0fcda5c25a69a670678c623e Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Wed, 18 Mar 2020 17:20:23 +0100
Subject: [PATCH] Added IRI_JOINTS_MODULE_NO_JOINT_STATES and
IRI_JOINTS_MODULE_TARGET_LOST module status
---
include/iri_joints_module/iri_joints_module.h | 4 +++-
src/iri_joints_client_alg_node.cpp | 1 +
src/iri_joints_module.cpp | 11 ++++++++---
3 files changed, 12 insertions(+), 4 deletions(-)
diff --git a/include/iri_joints_module/iri_joints_module.h b/include/iri_joints_module/iri_joints_module.h
index 56ba932..a4bb588 100644
--- a/include/iri_joints_module/iri_joints_module.h
+++ b/include/iri_joints_module/iri_joints_module.h
@@ -61,7 +61,9 @@ typedef enum {IRI_JOINTS_MODULE_RUNNING,
IRI_JOINTS_MODULE_PREEMPTED,
IRI_JOINTS_MODULE_REJECTED,
IRI_JOINTS_MODULE_SET_PARAM_FAIL,
- IRI_JOINTS_MODULE_PARAM_NOT_PRESENT} iri_joints_module_status_t;
+ IRI_JOINTS_MODULE_PARAM_NOT_PRESENT,
+ IRI_JOINTS_MODULE_NO_JOINT_STATES,
+ IRI_JOINTS_MODULE_TARGET_LOST} iri_joints_module_status_t;
typedef enum {axis_x, axis_y, axis_z}Axis_enum_t;
/**
diff --git a/src/iri_joints_client_alg_node.cpp b/src/iri_joints_client_alg_node.cpp
index 1410587..8c7084d 100644
--- a/src/iri_joints_client_alg_node.cpp
+++ b/src/iri_joints_client_alg_node.cpp
@@ -106,6 +106,7 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
if (config.add_joint)
{
+ ROS_INFO_STREAM("Added joint");
config.add_joint = false;
this->joint_names.push_back(config.joint_name);
this->angles.push_back(config.joint_angle);
diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp
index 5084fab..63ce9b9 100644
--- a/src/iri_joints_module.cpp
+++ b/src/iri_joints_module.cpp
@@ -173,10 +173,10 @@ void CIriJointsModule::state_machine(void)
this->track_action.stop_timeout();
break;
case ACTION_TIMEOUT:
- ROS_ERROR("CIriJointsModule : PointHeadTrackerAction did not finish in the allowed time");
+ ROS_ERROR("CIriJointsModule : PointHeadTrackerAction target info not updated on the allowed time");
this->track_action.cancel();
this->state = IRI_JOINTS_MODULE_IDLE;
- this->status = IRI_JOINTS_MODULE_TIMEOUT;
+ this->status = IRI_JOINTS_MODULE_TARGET_LOST;
this->track_action.stop_timeout();
break;
case ACTION_FB_WATCHDOG:
@@ -282,11 +282,16 @@ void CIriJointsModule::state_machine(void)
}
break;
}
+ if (this->joint_states_watchdog.is_active())
+ {
+ this->status = IRI_JOINTS_MODULE_NO_JOINT_STATES;
+ ROS_ERROR("CIriJointsModule: No joint_states received.");
+ }
}
void CIriJointsModule::reconfigure_callback(iri_joints_module::IriJointsModuleConfig &config, uint32_t level)
{
- ROS_DEBUG("CIriJointsModule : reconfigure callback");
+ ROS_DEBUG("CIriJointsModule: reconfigure callback");
this->lock();
this->config=config;
/* set the module rate */
--
GitLab