diff --git a/humanoid_common/package.xml b/humanoid_common/package.xml
index 547264e27217e195e4ea712e1b3f9b6f36005d37..ea1ee83c3fa798caa081ee2af86300cf4b3954a3 100755
--- a/humanoid_common/package.xml
+++ b/humanoid_common/package.xml
@@ -54,7 +54,7 @@
<run_depend>grippers_client</run_depend>
<run_depend>smart_charger_client</run_depend>
<run_depend>automatic_charge</run_depend>
-
+ <run_depend>stairs_client</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/humanoid_modules/cfg/StairsModule.cfg b/humanoid_modules/cfg/StairsModule.cfg
index 0a7ac29b36ed45b18b86655f18685dee0f8301d0..7e84ae83fb6e620a89d3df45a404ed7d2130aac2 100755
--- a/humanoid_modules/cfg/StairsModule.cfg
+++ b/humanoid_modules/cfg/StairsModule.cfg
@@ -40,6 +40,7 @@ gen = ParameterGenerator()
set_modules = gen.add_group("Set Servo Modules service")
set_params = gen.add_group("Set Stairs parameters service")
get_params = gen.add_group("Get Stairs parameters service")
+stairs_action = gen.add_group("Climb stairs")
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
@@ -52,4 +53,9 @@ set_params.add("set_params_max_retries",int_t, 0, "
get_params.add("get_params_max_retries",int_t, 0, "Maximum number of retries for the get params service", 1, 1, 10)
+stairs_action.add("action_max_retries",int_t, 0, "Maximum number of retries fro the action start", 1, 1, 10)
+stairs_action.add("feedback_watchdog_time_s",double_t, 0, "Maximum time between feedback messages", 1, 0.01, 10)
+stairs_action.add("timeout_s", double_t, 0, "Maximum time allowed to complete the action", 1, 0.1, 30)
+stairs_action.add("enable_timeout",bool_t, 0, "Enable joints timeout ", True)
+
exit(gen.generate(PACKAGE, "StairsModule", "StairsModule"))
diff --git a/humanoid_modules/include/humanoid_modules/stairs_module.h b/humanoid_modules/include/humanoid_modules/stairs_module.h
index 90d46f64261df4fc1179895eefe6d9ce577284b3..1962587308f55d2ce8336a3616f26170c3df21fe 100644
--- a/humanoid_modules/include/humanoid_modules/stairs_module.h
+++ b/humanoid_modules/include/humanoid_modules/stairs_module.h
@@ -17,11 +17,20 @@ typedef enum {STAIRS_MODULE_SERVO_ASSIGN_FAIL,
STAIRS_MODULE_NOT_INITIALIZED,
STAIRS_MODULE_SET_PARAMS_FAIL,
STAIRS_MODULE_ROBOT_FELL,
- STAIRS_MODULE_SUCCESS} stairs_module_status_t;
+ STAIRS_MODULE_RUNNING,
+ STAIRS_MODULE_SUCCESS,
+ STAIRS_MODULE_ACTION_START_FAIL,
+ STAIRS_MODULE_TIMEOUT,
+ STAIRS_MODULE_FB_WATCHDOG,
+ STAIRS_MODULE_ABORTED,
+ STAIRS_MODULE_PREEMPTED} stairs_module_status_t;
class CStairsModule : public CHumanoidModule<stairs_module::StairsModuleConfig>
{
private:
+ /* stairs action */
+ CModuleAction<humanoid_common_msgs::humanoid_stairsAction> climb_stairs;
+ humanoid_common_msgs::humanoid_stairsGoal goal;
/* set stairs parameters service */
CModuleService<humanoid_common_msgs::set_stairs_params> set_stairs_params_service;
CModuleService<humanoid_common_msgs::get_stairs_params> get_stairs_params_service;
@@ -44,19 +53,20 @@ class CStairsModule : public CHumanoidModule<stairs_module::StairsModuleConfig>
public:
CStairsModule(const std::string &name);
/* control functions */
+ void start(bool up);
void stop(void);
bool is_finished(void);
stairs_module_status_t get_status(void);
/* configuration parameters */
- void set_phase_time(stairs_phase_t pahse_id, double time);
+ void set_phase_time(stairs_phase_t phase_id, double time);
void set_x_offset(double offset);
void set_y_offset(double offset);
void set_z_offset(double offset);
void set_roll_offset(double offset);
void set_pitch_offset(double offset);
void set_yaw_offset(double offset);
- void set_y_shift(double shift);
- void set_x_shift(double shift);
+ void set_y_shift(double distance);
+ void set_x_shift(double distance);
void set_z_overshoot(double overshoot);
void set_stair_height(double height);
void set_hip_pitch_offset(double offset);
diff --git a/humanoid_modules/src/joints_module.cpp b/humanoid_modules/src/joints_module.cpp
index b56f64f277a34844f416830ecd8b092abd8c1e52..ee872b5c1d9c329deec3b4e412431658d8c1d242 100644
--- a/humanoid_modules/src/joints_module.cpp
+++ b/humanoid_modules/src/joints_module.cpp
@@ -290,9 +290,3 @@ CJointsModule::~CJointsModule()
while(!this->joints_trajectory.is_finished());
}
}
-/*
-Prueba
-{
- CJointsModule
-
-}*/
diff --git a/humanoid_modules/src/stairs_module.cpp b/humanoid_modules/src/stairs_module.cpp
index 8a5cfa7a8f04183d38ca4bd8319a452b43395182..43cf9362a54aa86ae508feba5c2a480a913a52c9 100644
--- a/humanoid_modules/src/stairs_module.cpp
+++ b/humanoid_modules/src/stairs_module.cpp
@@ -2,7 +2,8 @@
CStairsModule::CStairsModule(const std::string &name) : CHumanoidModule(name,WALK_MODULE),
set_stairs_params_service("set_stairs_params",name),
- get_stairs_params_service("get_stairs_params",name)
+ get_stairs_params_service("get_stairs_params",name),
+ climb_stairs("climb_stairs",name)
{
this->start_operation();
this->state=STAIRS_MODULE_GET_PARAMS;
@@ -81,20 +82,63 @@ void CStairsModule::state_machine(void)
}
break;
case STAIRS_MODULE_START: ROS_INFO("CStairsModule : state START");
- if(this->cancel_pending)
+ switch(this->climb_stairs.make_request(this->goal))
{
- this->cancel_pending=false;
- this->state=STAIRS_MODULE_WAIT;
- }
- else if(this->fallen)
- {
- this->state=STAIRS_MODULE_WAIT;
- }
- else
- this->state=STAIRS_MODULE_START;
- break;
+ case ACT_SRV_SUCCESS: this->state=STAIRS_MODULE_WAIT;
+ ROS_DEBUG("CStairsModule : goal start successfull");
+ /* start timeout */
+ if(this->config.enable_timeout)
+ this->climb_stairs.start_timeout(this->config.timeout_s);
+ break;
+ case ACT_SRV_PENDING: this->state=STAIRS_MODULE_START;
+ ROS_WARN("CStairsModule : goal start pending");
+ break;
+ case ACT_SRV_FAIL: this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_ACTION_START_FAIL;
+ ROS_ERROR("CStairsModule : goal start failed");
+ break;
+ }
+ break;
case STAIRS_MODULE_WAIT: ROS_INFO("CStairsModule : state WAIT");
- this->state=STAIRS_MODULE_IDLE;
+ switch(this->climb_stairs.get_state())
+ {
+ case ACTION_IDLE:
+ case ACTION_RUNNING: ROS_DEBUG("CStairsModules : action running");
+ this->state=STAIRS_MODULE_WAIT;
+ break;
+ case ACTION_SUCCESS: ROS_INFO("CStairsModules : action ended successfully");
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_SUCCESS;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_TIMEOUT: ROS_ERROR("CStairsModules : action did not finish in the allowed time");
+ this->climb_stairs.cancel();
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_TIMEOUT;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_FB_WATCHDOG: ROS_ERROR("CStairsModules : No feeback received for a long time");
+ this->climb_stairs.cancel();
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_FB_WATCHDOG;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_ABORTED: ROS_ERROR("CStairsModules : Action failed to complete");
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_ABORTED;
+ this->climb_stairs.stop_timeout();
+ break;
+ case ACTION_PREEMPTED: ROS_ERROR("CStairsModules : Action was interrupted by another request");
+ this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_PREEMPTED;
+ this->climb_stairs.stop_timeout();
+ break;
+ }
+ if(this->cancel_pending)
+ {
+ this->cancel_pending=false;
+ this->climb_stairs.cancel();
+ }
break;
}
if(this->update_parameters)
@@ -110,7 +154,10 @@ void CStairsModule::state_machine(void)
case ACT_SRV_FAIL: this->status=STAIRS_MODULE_SET_PARAMS_FAIL;
this->update_parameters=false;
if(this->state!=STAIRS_MODULE_GET_PARAMS && this->state!=STAIRS_MODULE_IDLE)
+ {
+ this->climb_stairs.cancel();
this->state=STAIRS_MODULE_WAIT;
+ }
ROS_ERROR("CStairsModule: Impossible to set stairs parameters");
break;
}
@@ -130,6 +177,13 @@ void CStairsModule::reconfigure_callback(stairs_module::StairsModuleConfig &conf
this->set_stairs_params_service.set_max_num_retries(config.set_params_max_retries);
/* get stairs parameters service parameters */
this->get_stairs_params_service.set_max_num_retries(config.get_params_max_retries);
+ /* stairs action parameters */
+ this->climb_stairs.set_max_num_retries(config.action_max_retries);
+ this->climb_stairs.set_feedback_watchdog_time(config.feedback_watchdog_time_s);
+ if(this->config.enable_timeout)
+ this->climb_stairs.enable_timeout();
+ else
+ this->climb_stairs.disable_timeout();
this->unlock();
}
@@ -146,6 +200,17 @@ void CStairsModule::fallen_state_callback(const std_msgs::Int8::ConstPtr& msg)
}
/* control functions */
+void CStairsModule::start(bool up)
+{
+ this->lock();
+ if(this->state==STAIRS_MODULE_IDLE)
+ {
+ this->goal.up=up;
+ this->start_climbing=true;
+ }
+ this->unlock();
+}
+
void CStairsModule::stop(void)
{
if(this->state!=STAIRS_MODULE_IDLE && this->state!=STAIRS_MODULE_WAIT)
@@ -168,7 +233,168 @@ bool CStairsModule::is_finished(void)
}
/* configuration parameters */
+void CStairsModule::set_phase_time(stairs_phase_t phase_id, double time)
+{
+ this->lock();
+ switch(phase_id)
+ {
+ case SHIFT_WEIGHT_LEFT: this->stairs_params.PHASE1_TIME=time;
+ break;
+ case RISE_RIGHT_FOOT: this->stairs_params.PHASE2_TIME=time;
+ break;
+ case ADVANCE_RIGHT_FOOT: this->stairs_params.PHASE3_TIME=time;
+ break;
+ case CONTACT_RIGHT_FOOT: this->stairs_params.PHASE4_TIME=time;
+ break;
+ case SHIFT_WEIGHT_RIGHT: this->stairs_params.PHASE5_TIME=time;
+ break;
+ case RISE_LEFT_FOOT: this->stairs_params.PHASE6_TIME=time;
+ break;
+ case ADVANCE_LEFT_FOOT: this->stairs_params.PHASE7_TIME=time;
+ break;
+ case CONTACT_LEFT_FOOT: this->stairs_params.PHASE8_TIME=time;
+ break;
+ case CENTER_WEIGHT: this->stairs_params.PHASE9_TIME=time;
+ break;
+ }
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_x_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.X_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_y_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.Y_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_z_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.Z_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_roll_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.R_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_pitch_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.P_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_yaw_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.A_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_y_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.Y_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_x_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.X_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_z_overshoot(double overshoot)
+{
+ this->lock();
+ this->stairs_params.Z_OVERSHOOT=overshoot;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_stair_height(double height)
+{
+ this->lock();
+ this->stairs_params.Z_HEIGHT=height;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_hip_pitch_offset(double offset)
+{
+ this->lock();
+ this->stairs_params.HIP_PITCH_OFFSET=offset;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_roll_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.R_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_pitch_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.P_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_yaw_shift(double distance)
+{
+ this->lock();
+ this->stairs_params.A_SHIFT=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_y_spread(double distance)
+{
+ this->lock();
+ this->stairs_params.Y_SPREAD=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
+void CStairsModule::set_x_shift_body(double distance)
+{
+ this->lock();
+ this->stairs_params.X_SHIFT_BODY=distance;
+ this->update_parameters=true;
+ this->unlock();
+}
+
CStairsModule::~CStairsModule()
{
+ if(!this->climb_stairs.is_finished())
+ {
+ this->climb_stairs.cancel();
+ while(!this->climb_stairs.is_finished());
+ }
}