diff --git a/include/iri_joints_module/iri_joints_module.h b/include/iri_joints_module/iri_joints_module.h index 56ba932c4b63f26372929aeccba0f973db88f753..a4bb588554542ebbb0963ef82ccc658708d96fda 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 141058723b7ff6bef9f265e8c11cc9a1f03e9625..8c7084d875288f6c274fb34a4f74bc9dc2947563 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 5084fab7c0eb9018f989c32467a05045e0042bba..63ce9b94da789432e7adf6b6c8a7151e60d34cc2 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 */