Skip to content
Snippets Groups Projects
Commit 8ce2b7f4 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added Action enable

parent a8c2d9f3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
cfg/IriJointsModule.cfg
+ 8
− 5
View file @ 8ce2b7f4
......@@ -57,6 +57,7 @@ joint_states.add("joint_states_watchdog_time_s", double_t, 0,
point_to.add("pointing_frame", str_t, 0, "The pointing frame")
point_to.add("pointing_axis", int_t, 0, "The pointing axis", 2, 0, 2, edit_method=axis_enum)
point_to.add("point_frame", str_t, 0, "The point's frame")
point_to.add("pt_enabled", bool_t, 0, "Enable pointTo action execution", True)
point_to.add("pt_max_retries", int_t, 0, "Maximum number of retries for the action start", 1, 1, 10)
point_to.add("pt_enable_watchdog", bool_t, 0, "Enable action watchdog", False)
......@@ -64,17 +65,19 @@ point_to.add("pt_feedback_watchdog_time_s", double_t, 0,
point_to.add("pt_enable_timeout", bool_t, 0, "Enable action timeout", True)
point_to.add("pt_timeout_s", double_t, 0, "Maximum time allowed to complete the action", 5, 0.1, 6000)
tracker.add("tracker_enabled", bool_t, 0, "Enable pointTo tracker action execution", True)
tracker.add("tracker_max_retries", int_t, 0, "Maximum number of retries for the action start", 1, 1, 10)
tracker.add("tracker_enable_watchdog", bool_t, 0, "Enable action watchdog", True)
tracker.add("tracker_feedback_watchdog_time_s", double_t, 0, "Maximum time between feedback messages", 0.5, 0.01, 600)
tracker.add("tracker_enable_timeout", bool_t, 0, "Enable action timeout", True)
tracker.add("tracker_timeout_s", double_t, 0, "Maximum time allowed to complete the action", 5, 0.1, 6000)
move_to_angles.add("move_to_angles_time_from_start", int_t, 0, "Max time in seconds to finish the action", 10, 0, 100)
move_to_angles.add("move_to_angles_pos_tol", double_t, 0, "Path and goal position tolerance in rad or m", 0.002, -1)
move_to_angles.add("move_to_angles_vel_tol", double_t, 0, "Path and goal velocity tolerance in rad/s or m/s", 0.0, -1)
move_to_angles.add("move_to_angles_acc_tol", double_t, 0, "Path and goal acceleration tolerance in rad/s2 or m/s2", 0.0, -1)
move_to_angles.add("move_to_angles_time_tol", double_t, 0, "Path and goal time tolerance in percentaje", 0.1, 0.0, 1.0)
move_to_angles.add("move_to_angles_time_from_start", int_t, 0, "Max time in seconds to finish the action", 10, 0, 100)
move_to_angles.add("move_to_angles_pos_tol", double_t, 0, "Path and goal position tolerance in rad or m", 0.002, -1)
move_to_angles.add("move_to_angles_vel_tol", double_t, 0, "Path and goal velocity tolerance in rad/s or m/s", 0.0, -1)
move_to_angles.add("move_to_angles_acc_tol", double_t, 0, "Path and goal acceleration tolerance in rad/s2 or m/s2", 0.0, -1)
move_to_angles.add("move_to_angles_time_tol", double_t, 0, "Path and goal time tolerance in percentaje", 0.1, 0.0, 1.0)
move_to_angles.add("move_to_angles_enabled", bool_t, 0, "Enable move to angles action execution", True)
move_to_angles.add("move_to_angles_max_retries", int_t, 0, "Maximum number of retries for the action start", 1, 1, 10)
move_to_angles.add("move_to_angles_enable_watchdog", bool_t, 0, "Enable action watchdog", True)
......
config/joints_module_default.yaml
+ 3
− 0
View file @ 8ce2b7f4
......@@ -6,6 +6,7 @@ joint_states_watchdog_time_s: 1.0
pointing_frame: "camera_optical_frame"
pointing_axis: 2
point_frame: "world"
pt_enabled: True
pt_max_retries: 1
pt_enable_watchdog: False
......@@ -13,6 +14,7 @@ pt_feedback_watchdog_time_s: 600.0
pt_enable_timeout: True
pt_timeout_s: 25.0
tracker_enabled: True
tracker_max_retries: 1
tracker_enable_watchdog: True
tracker_feedback_watchdog_time_s: 1.0
......@@ -24,6 +26,7 @@ move_to_angles_pos_tol: 0.02
move_to_angles_vel_tol: -1.0
move_to_angles_acc_tol: -1.0
move_to_angles_time_tol: 0.0
move_to_angles_enabled: True
move_to_angles_max_retries: 1
move_to_angles_enable_watchdog: True
......
src/iri_joints_module.cpp
+ 30
− 0
View file @ 8ce2b7f4
......@@ -341,6 +341,12 @@ void CIriJointsModule::reconfigure_callback(iri_joints_module::IriJointsModuleCo
else
this->point_to_action.disable_timeout();
if(this->config.pt_enabled)
this->point_to_action.enable();
else
this->point_to_action.disable();
/*Tracker Action parameters*/
this->track_action.set_max_num_retries(config.tracker_max_retries);
if(config.tracker_enable_watchdog)
......@@ -352,6 +358,11 @@ void CIriJointsModule::reconfigure_callback(iri_joints_module::IriJointsModuleCo
else
this->track_action.disable_timeout();
if(this->config.tracker_enabled)
this->track_action.enable();
else
this->track_action.disable();
/*Move to angles Action parameters*/
this->move_to_angle_action.set_max_num_retries(config.move_to_angles_max_retries);
if(config.move_to_angles_enable_watchdog)
......@@ -363,6 +374,11 @@ void CIriJointsModule::reconfigure_callback(iri_joints_module::IriJointsModuleCo
else
this->move_to_angle_action.disable_timeout();
if(this->config.move_to_angles_enabled)
this->move_to_angle_action.enable();
else
this->move_to_angle_action.disable();
this->unlock();
}
......@@ -411,6 +427,8 @@ bool CIriJointsModule::point_to(double x, double y, double z, double max_vel)
void CIriJointsModule::cancel_point_to(void)
{
if (!point_to_action.is_enabled())
return;
if(this->state == IRI_JOINTS_MODULE_POINT_WAIT)
this->cancel_point_to_pending = true;
else
......@@ -419,6 +437,8 @@ void CIriJointsModule::cancel_point_to(void)
bool CIriJointsModule::is_point_to_finished(void)
{
if (!point_to_action.is_enabled())
return true;
this->lock();
if(this->state == IRI_JOINTS_MODULE_POINT_WAIT || (this->state == IRI_JOINTS_MODULE_IDLE && this->new_point_goal))
{
......@@ -435,6 +455,8 @@ bool CIriJointsModule::is_point_to_finished(void)
/*Tracking action*/
bool CIriJointsModule::target_update(double x, double y, double z)
{
if (!track_action.is_enabled())
return true;
this->lock();
if (this->state == IRI_JOINTS_MODULE_POINT_WAIT)
{
......@@ -488,6 +510,8 @@ bool CIriJointsModule::enable_tracker(void)
void CIriJointsModule::cancel_tracker(void)
{
if (!track_action.is_enabled())
return;
if(this->state == IRI_JOINTS_MODULE_TRACKING)
this->cancel_tracker_pending = true;
else
......@@ -496,6 +520,8 @@ void CIriJointsModule::cancel_tracker(void)
bool CIriJointsModule::is_tracker_finished(void)
{
if (!track_action.is_enabled())
return true;
this->lock();
if(this->state == IRI_JOINTS_MODULE_TRACKING || (this->state == IRI_JOINTS_MODULE_IDLE && this->enable_tracker_pending))
{
......@@ -570,6 +596,8 @@ bool CIriJointsModule::move_to_angles(std::vector<std::string> joint_names, std:
void CIriJointsModule::cancel_move_to_angles(void)
{
if (!move_to_angle_action.is_enabled())
return;
if(this->state == IRI_JOINTS_MODULE_MOVE_ANGLES)
this->cancel_move_to_angles_pending = true;
else
......@@ -578,6 +606,8 @@ void CIriJointsModule::cancel_move_to_angles(void)
bool CIriJointsModule::is_move_to_angles_finished(void)
{
if (!move_to_angle_action.is_enabled())
return true;
this->lock();
if(this->state == IRI_JOINTS_MODULE_MOVE_ANGLES || (this->state == IRI_JOINTS_MODULE_IDLE && this->new_angles))
{
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment