Skip to content
Snippets Groups Projects
Commit c04fe493 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Completed and tested the stairs module.

parent 1cada630
No related branches found
No related tags found
1 merge request!4Filtered localization
Hide whitespace changes
Inline Side-by-side
humanoid_common/package.xml
+ 1
− 1
View file @ c04fe493
...@@ -54,7 +54,7 @@ ...@@ -54,7 +54,7 @@
<run_depend>grippers_client</run_depend> <run_depend>grippers_client</run_depend>
<run_depend>smart_charger_client</run_depend> <run_depend>smart_charger_client</run_depend>
<run_depend>automatic_charge</run_depend> <run_depend>automatic_charge</run_depend>
<run_depend>stairs_client</run_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
......
humanoid_modules/cfg/StairsModule.cfg
+ 6
− 0
View file @ c04fe493
...@@ -40,6 +40,7 @@ gen = ParameterGenerator() ...@@ -40,6 +40,7 @@ gen = ParameterGenerator()
set_modules = gen.add_group("Set Servo Modules service") set_modules = gen.add_group("Set Servo Modules service")
set_params = gen.add_group("Set Stairs parameters service") set_params = gen.add_group("Set Stairs parameters service")
get_params = gen.add_group("Get 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 # 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) #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, " ...@@ -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) 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")) exit(gen.generate(PACKAGE, "StairsModule", "StairsModule"))
humanoid_modules/include/humanoid_modules/stairs_module.h
+ 14
− 4
View file @ c04fe493
...@@ -17,11 +17,20 @@ typedef enum {STAIRS_MODULE_SERVO_ASSIGN_FAIL, ...@@ -17,11 +17,20 @@ typedef enum {STAIRS_MODULE_SERVO_ASSIGN_FAIL,
STAIRS_MODULE_NOT_INITIALIZED, STAIRS_MODULE_NOT_INITIALIZED,
STAIRS_MODULE_SET_PARAMS_FAIL, STAIRS_MODULE_SET_PARAMS_FAIL,
STAIRS_MODULE_ROBOT_FELL, 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> class CStairsModule : public CHumanoidModule<stairs_module::StairsModuleConfig>
{ {
private: private:
/* stairs action */
CModuleAction<humanoid_common_msgs::humanoid_stairsAction> climb_stairs;
humanoid_common_msgs::humanoid_stairsGoal goal;
/* set stairs parameters service */ /* set stairs parameters service */
CModuleService<humanoid_common_msgs::set_stairs_params> set_stairs_params_service; CModuleService<humanoid_common_msgs::set_stairs_params> set_stairs_params_service;
CModuleService<humanoid_common_msgs::get_stairs_params> get_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> ...@@ -44,19 +53,20 @@ class CStairsModule : public CHumanoidModule<stairs_module::StairsModuleConfig>
public: public:
CStairsModule(const std::string &name); CStairsModule(const std::string &name);
/* control functions */ /* control functions */
void start(bool up);
void stop(void); void stop(void);
bool is_finished(void); bool is_finished(void);
stairs_module_status_t get_status(void); stairs_module_status_t get_status(void);
/* configuration parameters */ /* 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_x_offset(double offset);
void set_y_offset(double offset); void set_y_offset(double offset);
void set_z_offset(double offset); void set_z_offset(double offset);
void set_roll_offset(double offset); void set_roll_offset(double offset);
void set_pitch_offset(double offset); void set_pitch_offset(double offset);
void set_yaw_offset(double offset); void set_yaw_offset(double offset);
void set_y_shift(double shift); void set_y_shift(double distance);
void set_x_shift(double shift); void set_x_shift(double distance);
void set_z_overshoot(double overshoot); void set_z_overshoot(double overshoot);
void set_stair_height(double height); void set_stair_height(double height);
void set_hip_pitch_offset(double offset); void set_hip_pitch_offset(double offset);
......
humanoid_modules/src/joints_module.cpp
+ 0
− 6
View file @ c04fe493
...@@ -290,9 +290,3 @@ CJointsModule::~CJointsModule() ...@@ -290,9 +290,3 @@ CJointsModule::~CJointsModule()
while(!this->joints_trajectory.is_finished()); while(!this->joints_trajectory.is_finished());
} }
} }
/*
Prueba
{
CJointsModule
}*/
humanoid_modules/src/stairs_module.cpp
+ 239
− 13
View file @ c04fe493
...@@ -2,7 +2,8 @@ ...@@ -2,7 +2,8 @@
CStairsModule::CStairsModule(const std::string &name) : CHumanoidModule(name,WALK_MODULE), CStairsModule::CStairsModule(const std::string &name) : CHumanoidModule(name,WALK_MODULE),
set_stairs_params_service("set_stairs_params",name), 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->start_operation();
this->state=STAIRS_MODULE_GET_PARAMS; this->state=STAIRS_MODULE_GET_PARAMS;
...@@ -81,20 +82,63 @@ void CStairsModule::state_machine(void) ...@@ -81,20 +82,63 @@ void CStairsModule::state_machine(void)
} }
break; break;
case STAIRS_MODULE_START: ROS_INFO("CStairsModule : state START"); 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; case ACT_SRV_SUCCESS: this->state=STAIRS_MODULE_WAIT;
this->state=STAIRS_MODULE_WAIT; ROS_DEBUG("CStairsModule : goal start successfull");
} /* start timeout */
else if(this->fallen) if(this->config.enable_timeout)
{ this->climb_stairs.start_timeout(this->config.timeout_s);
this->state=STAIRS_MODULE_WAIT; break;
} case ACT_SRV_PENDING: this->state=STAIRS_MODULE_START;
else ROS_WARN("CStairsModule : goal start pending");
this->state=STAIRS_MODULE_START; break;
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"); 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; break;
} }
if(this->update_parameters) if(this->update_parameters)
...@@ -110,7 +154,10 @@ void CStairsModule::state_machine(void) ...@@ -110,7 +154,10 @@ void CStairsModule::state_machine(void)
case ACT_SRV_FAIL: this->status=STAIRS_MODULE_SET_PARAMS_FAIL; case ACT_SRV_FAIL: this->status=STAIRS_MODULE_SET_PARAMS_FAIL;
this->update_parameters=false; this->update_parameters=false;
if(this->state!=STAIRS_MODULE_GET_PARAMS && this->state!=STAIRS_MODULE_IDLE) if(this->state!=STAIRS_MODULE_GET_PARAMS && this->state!=STAIRS_MODULE_IDLE)
{
this->climb_stairs.cancel();
this->state=STAIRS_MODULE_WAIT; this->state=STAIRS_MODULE_WAIT;
}
ROS_ERROR("CStairsModule: Impossible to set stairs parameters"); ROS_ERROR("CStairsModule: Impossible to set stairs parameters");
break; break;
} }
...@@ -130,6 +177,13 @@ void CStairsModule::reconfigure_callback(stairs_module::StairsModuleConfig &conf ...@@ -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); this->set_stairs_params_service.set_max_num_retries(config.set_params_max_retries);
/* get stairs parameters service parameters */ /* get stairs parameters service parameters */
this->get_stairs_params_service.set_max_num_retries(config.get_params_max_retries); 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(); this->unlock();
} }
...@@ -146,6 +200,17 @@ void CStairsModule::fallen_state_callback(const std_msgs::Int8::ConstPtr& msg) ...@@ -146,6 +200,17 @@ void CStairsModule::fallen_state_callback(const std_msgs::Int8::ConstPtr& msg)
} }
/* control functions */ /* 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) void CStairsModule::stop(void)
{ {
if(this->state!=STAIRS_MODULE_IDLE && this->state!=STAIRS_MODULE_WAIT) if(this->state!=STAIRS_MODULE_IDLE && this->state!=STAIRS_MODULE_WAIT)
...@@ -168,7 +233,168 @@ bool CStairsModule::is_finished(void) ...@@ -168,7 +233,168 @@ bool CStairsModule::is_finished(void)
} }
/* configuration parameters */ /* 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() CStairsModule::~CStairsModule()
{ {
if(!this->climb_stairs.is_finished())
{
this->climb_stairs.cancel();
while(!this->climb_stairs.is_finished());
}
} }
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