diff --git a/humanoid_modules/CMakeLists.txt b/humanoid_modules/CMakeLists.txt
index 5a906ecfb54513a58bb16f45ad40f49315211b5f..e2e7111ac076396b3da08cfa4fcb7a57054ab842 100644
--- a/humanoid_modules/CMakeLists.txt
+++ b/humanoid_modules/CMakeLists.txt
@@ -86,7 +86,7 @@ find_package(iriutils REQUIRED)
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
- cfg/ActionModule.cfg cfg/JointsCartModule.cfg cfg/WalkModule.cfg cfg/HeadTrackingModule.cfg cfg/JointsModule.cfg cfg/SearchModule.cfg cfg/TrackingModule.cfg cfg/GripperModule.cfg
+ cfg/ActionModule.cfg cfg/JointsCartModule.cfg cfg/WalkModule.cfg cfg/HeadTrackingModule.cfg cfg/JointsModule.cfg cfg/SearchModule.cfg cfg/TrackingModule.cfg cfg/GripperModule.cfg cfg/StairsModule.cfg
)
###################################
@@ -100,7 +100,7 @@ generate_dynamic_reconfigure_options(
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module search_module tracking_module gripper_module
+ LIBRARIES action_module joints_cart_module walk_module head_tracking_module joints_module search_module tracking_module gripper_module stairs_module
CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools humanoid_common_msgs actionlib control_msgs geometry_msgs sensor_msgs trajectory_msgs std_msgs tf
# DEPENDS system_lib
)
@@ -216,6 +216,18 @@ target_link_libraries(gripper_module ${iriutils_LIBRARY})
# ## either from message generation or dynamic reconfigure
add_dependencies(gripper_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_library(stairs_module
+ src/stairs_module.cpp
+)
+
+target_link_libraries(stairs_module ${catkin_LIBRARIES})
+target_link_libraries(stairs_module ${iriutils_LIBRARY})
+#
+# ## Add cmake target dependencies of the library
+# ## as an example, code may need to be generated before libraries
+# ## either from message generation or dynamic reconfigure
+add_dependencies(stairs_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
diff --git a/humanoid_modules/cfg/StairsModule.cfg b/humanoid_modules/cfg/StairsModule.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..0a7ac29b36ed45b18b86655f18685dee0f8301d0
--- /dev/null
+++ b/humanoid_modules/cfg/StairsModule.cfg
@@ -0,0 +1,55 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='stairs_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+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")
+
+# 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("rate_hz", double_t, 0, "Action module rate in Hz", 1.0, 1.0, 100.0)
+gen.add("enable", bool_t, 0, "Enable action execution", True)
+
+set_modules.add("set_modules_max_retries",int_t, 0, "Maximum number of retries for the set modules service", 1, 1, 10)
+
+set_params.add("set_params_max_retries",int_t, 0, "Maximum number of retries for the set 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)
+
+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
new file mode 100644
index 0000000000000000000000000000000000000000..90d46f64261df4fc1179895eefe6d9ce577284b3
--- /dev/null
+++ b/humanoid_modules/include/humanoid_modules/stairs_module.h
@@ -0,0 +1,72 @@
+#ifndef _STAIRS_MODULE_H
+#define _STAIRS_MODULE_H
+
+#include <humanoid_modules/humanoid_module.h>
+#include <humanoid_modules/StairsModuleConfig.h>
+#include <iri_ros_tools/module_service.h>
+#include <humanoid_common_msgs/set_stairs_params.h>
+#include <humanoid_common_msgs/get_stairs_params.h>
+#include <humanoid_common_msgs/humanoid_stairsAction.h>
+#include <std_msgs/Int8.h>
+
+typedef enum {STAIRS_MODULE_GET_PARAMS,STAIRS_MODULE_IDLE,STAIRS_MODULE_SET_MODULES,STAIRS_MODULE_START,STAIRS_MODULE_WAIT} stairs_module_state_t;
+
+typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
+
+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;
+
+class CStairsModule : public CHumanoidModule<stairs_module::StairsModuleConfig>
+{
+ private:
+ /* 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;
+ humanoid_common_msgs::stairs_params stairs_params;
+ bool update_parameters;
+ /* fallen state subscriber */
+ ros::Subscriber fallen_state_subscriber;
+ void fallen_state_callback(const std_msgs::Int8::ConstPtr& msg);
+ bool fallen;
+ /* general attributes */
+ stairs_module::StairsModuleConfig config;
+ stairs_module_state_t state;
+ stairs_module_status_t status;
+ bool cancel_pending;
+ /* goal */
+ bool start_climbing;
+ protected:
+ void state_machine(void);
+ void reconfigure_callback(stairs_module::StairsModuleConfig &config, uint32_t level);
+ public:
+ CStairsModule(const std::string &name);
+ /* control functions */
+ 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_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_z_overshoot(double overshoot);
+ void set_stair_height(double height);
+ void set_hip_pitch_offset(double offset);
+ void set_roll_shift(double distance);
+ void set_pitch_shift(double distance);
+ void set_yaw_shift(double distance);
+ void set_y_spread(double distance);
+ void set_x_shift_body(double distance);
+ /* feedback functions */
+ ~CStairsModule();
+};
+
+#endif
diff --git a/humanoid_modules/src/stairs_module.cpp b/humanoid_modules/src/stairs_module.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a5cfa7a8f04183d38ca4bd8319a452b43395182
--- /dev/null
+++ b/humanoid_modules/src/stairs_module.cpp
@@ -0,0 +1,174 @@
+#include <humanoid_modules/stairs_module.h>
+
+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)
+{
+ this->start_operation();
+ this->state=STAIRS_MODULE_GET_PARAMS;
+ this->status=STAIRS_MODULE_NOT_INITIALIZED;
+ this->cancel_pending=false;
+ this->start_climbing=false;
+ this->fallen=false;
+ this->update_parameters=false;
+ /* fallen state subscriber */
+ this->fallen_state_subscriber=this->module_nh.subscribe("fallen_state",1,&CStairsModule::fallen_state_callback,this);
+ this->add_left_leg();
+ this->add_right_leg();
+}
+
+void CStairsModule::state_machine(void)
+{
+ humanoid_common_msgs::get_stairs_params get_stairs_params;
+ humanoid_common_msgs::set_stairs_params set_stairs_params;
+
+ switch(this->state)
+ {
+ case STAIRS_MODULE_GET_PARAMS: ROS_INFO("CStairsModule : state GET_PARAMS");
+ switch(this->get_stairs_params_service.call(get_stairs_params))
+ {
+ case ACT_SRV_SUCCESS: this->state=STAIRS_MODULE_IDLE;
+ this->stairs_params=get_stairs_params.response.params;
+ this->status=STAIRS_MODULE_SUCCESS;
+ ROS_DEBUG("CStairsModule: Got the stairs parameters successfully");
+ break;
+ case ACT_SRV_PENDING: this->state=STAIRS_MODULE_GET_PARAMS;
+ ROS_WARN("CStairsModule: Still waiting to get the stairs parameters");
+ break;
+ case ACT_SRV_FAIL: this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_NOT_INITIALIZED;
+ ROS_ERROR("CStairsModule: Impossible to get the stairs parameters");
+ break;
+ }
+ break;
+ case STAIRS_MODULE_IDLE: ROS_INFO("CStairsModule : state IDLE");
+ if(this->start_climbing)
+ {
+ this->start_climbing=false;
+ this->cancel_pending=false;
+ if(this->status!=STAIRS_MODULE_NOT_INITIALIZED)
+ {
+ if(this->fallen)
+ {
+ this->status=STAIRS_MODULE_ROBOT_FELL;
+ this->state=STAIRS_MODULE_IDLE;
+ }
+ else
+ {
+ this->state=STAIRS_MODULE_SET_MODULES;
+ this->status=STAIRS_MODULE_SUCCESS;
+ }
+ }
+ else
+ this->state=STAIRS_MODULE_IDLE;
+ }
+ else
+ this->state=STAIRS_MODULE_IDLE;
+ break;
+ case STAIRS_MODULE_SET_MODULES: ROS_INFO("CStairsModule : state SET_MODULES");
+ switch(this->assign_servos())
+ {
+ case ACT_SRV_SUCCESS: this->state=STAIRS_MODULE_START;
+ ROS_DEBUG("CStairsModule: servos assigned successfully");
+ break;
+ case ACT_SRV_PENDING: this->state=STAIRS_MODULE_SET_MODULES;
+ ROS_WARN("CStairsModule: servo assigment pending");
+ break;
+ case ACT_SRV_FAIL: this->state=STAIRS_MODULE_IDLE;
+ this->status=STAIRS_MODULE_SET_PARAMS_FAIL;
+ ROS_ERROR("CStairsModule: servo_assigment failed");
+ break;
+ }
+ break;
+ case STAIRS_MODULE_START: ROS_INFO("CStairsModule : state START");
+ if(this->cancel_pending)
+ {
+ 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 STAIRS_MODULE_WAIT: ROS_INFO("CStairsModule : state WAIT");
+ this->state=STAIRS_MODULE_IDLE;
+ break;
+ }
+ if(this->update_parameters)
+ {
+ set_stairs_params.request.params=this->stairs_params;
+ switch(this->set_stairs_params_service.call(set_stairs_params))
+ {
+ case ACT_SRV_SUCCESS: ROS_DEBUG("CStairsModule: Walk parameters set successfully");
+ this->update_parameters=false;
+ break;
+ case ACT_SRV_PENDING: ROS_WARN("CStairsModule: Walk parameters not yet set");
+ break;
+ 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->state=STAIRS_MODULE_WAIT;
+ ROS_ERROR("CStairsModule: Impossible to set stairs parameters");
+ break;
+ }
+ }
+}
+
+void CStairsModule::reconfigure_callback(stairs_module::StairsModuleConfig &config, uint32_t level)
+{
+ ROS_INFO("CStairsModule : reconfigure callback");
+ this->lock();
+ this->config=config;
+ /* set the module rate */
+ this->set_rate(config.rate_hz);
+ /* set servo modules service parameters */
+ this->assign_service.set_max_num_retries(config.set_modules_max_retries);
+ /* set stairs parameters service parameters */
+ 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);
+ this->unlock();
+}
+
+void CStairsModule::fallen_state_callback(const std_msgs::Int8::ConstPtr& msg)
+{
+ this->lock();
+ if(msg->data==0 || msg->data==1)//forward fall or bacward fall
+ {
+ this->fallen=true;
+ }
+ else
+ this->fallen=false;
+ this->unlock();
+}
+
+/* control functions */
+void CStairsModule::stop(void)
+{
+ if(this->state!=STAIRS_MODULE_IDLE && this->state!=STAIRS_MODULE_WAIT)
+ {
+ this->cancel_pending=true;
+ }
+}
+
+stairs_module_status_t CStairsModule::get_status(void)
+{
+ return this->status;
+}
+
+bool CStairsModule::is_finished(void)
+{
+ if(this->state==STAIRS_MODULE_GET_PARAMS || (this->state==STAIRS_MODULE_IDLE && this->start_climbing==false))
+ return true;
+ else
+ return false;
+}
+
+/* configuration parameters */
+CStairsModule::~CStairsModule()
+{
+}
+